def motion(x, y):
global last
dx, dy = x - last[0], y - last[1]
q = quaternion.angvec2quat((dx**2 + dy**2)**.4/180*math.pi, [dy, dx, 0])
plot.Q = quaternion.multiply(q, plot.Q)
last = x, y
glutPostRedisplay()
def display_setup(self):
width, height = self.dim
ar = float(width) / float(height)
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
fac = .05
glFrustum(-fac * ar, fac * ar, -fac, fac, .1, 15)
glMatrixMode(GL_MODELVIEW)
glClearColor(0, 0, 0, 0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glPushMatrix()
s = self.userscale
glScalef(s, s, s)
TranslateAfter(0, 0, -1)
glPolygonMode(GL_FRONT_AND_BACK, self.mode)
glLineWidth(1)
glPushMatrix()
down = [0, 0, 1]
q = [1, 0, 0, 0]
q = quaternion.multiply(self.fusionQPose,
quaternion.conjugate(self.alignmentQ))
q = quaternion.normalize(q) # correct possible rounding errors
down = quaternion.rotvecquat(down, quaternion.conjugate(q))
glRotatef(-math.degrees(quaternion.angle(q)), *q[1:])
return down
def onMouseEventsBoatPlot(self, event):
self.BoatPlot.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
if self.lastmouse:
self.BoatPlot.Refresh()
dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1]
q = quaternion.angvec2quat((dx**2 + dy**2)**.4 / 180 * math.pi,
[dy, dx, 0])
self.boat_plot.Q = quaternion.multiply(q, self.boat_plot.Q)
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
while rotation > 0:
self.boat_plot.Scale /= .9
rotation -= 1
while rotation < 0:
self.boat_plot.Scale *= .9
rotation += 1
self.BoatPlot.Refresh()
def display(self, refresh):
self.bound_page()
self.fill(black)
self.fittext(rectangle(0, 0, 1, .24), _('Calibrate Info'), True)
if self.page == 0:
deviation = ['N/A', 'N/A']
deviationstr = 'N/A'
dim = '?'
try:
cal = self.last_val('imu.compass.calibration')
deviation = ['%.2f' % cal[1][0], '%.2f' % cal[1][1]]
dim = str(int(cal[2]))
#print(ndeviation)
names = [(0, _('incomplete')), (.01, _('excellent')),
(.02, _('good')), (.04, _('fair')), (.06, _('poor')),
(1000, _('bad'))]
for n in names:
if cal[1][0] <= n[0]:
deviationstr = n[1]
break
except:
pass
self.fittext(rectangle(0, .3, 1, .15), _('compass'))
self.fittext(rectangle(0, .42, 1, .23), deviationstr)
self.fittext(rectangle(0, .66, 1, .14),
deviation[0] + ' ' + dim + 'd')
self.fittext(rectangle(0, .8, 1, .2),
self.last_val('imu.compass.calibration.age')[:7])
elif self.page == 1:
try:
cal = self.last_val('imu.compass.calibration')
raw = ''
for c in cal[0]:
raw += '%.1f\n' % c
except:
raw = 'N/A'
self.fittext(rectangle(0, .3, 1, .7), raw)
else:
import math
mod = int(time.time() % 11) / 3
self.fittext(rectangle(0, .24, 1, .15), 'sigma plot')
cal = self.last_val('imu.compass.calibration')[0]
if len(cal) >= 5:
m = cal[3]
dip = math.radians(cal[4])
else:
m, dip = 0, 0 # not connected
if mod == 1:
m *= math.cos(dip)
try:
from pypilot import quaternion
p = self.last_val('imu.compass.calibration.sigmapoints')
q = self.last_val('imu.alignmentQ')
p = map(
lambda p0: map(lambda x0, c:
(x0 - c) / m, p0[:3], cal[:3]), p)
x, y, r = 24, 56, 20
if mod > 1:
if mod == 3:
x1, y1 = int(r * math.cos(dip)), int(r * math.sin(dip))
self.surface.line(x, y, x + x1, y + y1, white)
self.surface.line(x, y, x - x1, y + y1, white)
q = quaternion.multiply(
q, quaternion.angvec2quat(math.radians(90), [0, 1, 0]))
p = map(lambda p0: quaternion.rotvecquat(p0, q), p)
for p0 in p:
self.surface.putpixel(int(r * p0[0] + x),
int(r * p0[1] + y), white)
except:
self.fittext(rectangle(0, .3, 1, .7), 'N/A')
def display_calibrate_info(self):
if self.info_page > 2:
self.info_page = 0
elif self.info_page < 0:
self.info_page = 2
self.surface.fill(black)
self.fittext(rectangle(0, 0, 1, .24), _('Calibrate Info'), True)
if self.info_page == 0:
deviation = [_('N/A'), _('N/A')]
deviationstr = _('N/A')
dim = '?'
try:
cal = self.last_msg['imu.compass_calibration']
deviation = ['%.2f' % cal[1][0], '%.2f' % cal[1][1]]
dim = str(int(cal[2]))
#print ndeviation
names = [(0, _('incomplete')), (.01, _('excellent')),
(.02, _('good')), (.04, _('fair')), (.06, _('poor')),
(1000, _('bad'))]
for n in names:
if cal[1][0] <= n[0]:
deviationstr = n[1]
break
except:
pass
self.fittext(rectangle(0, .3, 1, .15), _('compass'))
self.fittext(rectangle(0, .42, 1, .23), deviationstr)
self.fittext(rectangle(0, .66, 1, .14),
deviation[0] + ' ' + dim + 'd')
self.fittext(rectangle(0, .8, 1, .2),
self.last_msg['imu.compass_calibration_age'][:7])
self.get('imu.compass_calibration_age')
elif self.info_page == 1:
try:
cal = self.last_msg['imu.compass_calibration']
raw = ''
for c in cal[0]:
raw += '%.1f\n' % c
except:
raw = 'N/A'
self.fittext(rectangle(0, .3, 1, .7), raw)
else:
mod = int(time.time() % 11) / 3
self.fittext(rectangle(0, .24, 1, .15), 'sigma plot')
cal = self.last_msg['imu.compass_calibration'][0]
m = cal[3]
dip = math.radians(cal[4])
if mod == 1:
m *= math.cos(dip)
try:
p = self.last_msg['imu.compass_calibration_sigmapoints']
q = self.last_msg['imu.alignmentQ']
p = map(
lambda p0: map(lambda x0, c:
(x0 - c) / m, p0[:3], cal[:3]), p)
x, y, r = 24, 56, 20
if mod > 1:
if mod == 3:
x1, y1 = int(r * math.cos(dip)), int(r * math.sin(dip))
self.surface.line(x, y, x + x1, y + y1, white)
self.surface.line(x, y, x - x1, y + y1, white)
q = quaternion.multiply(
q, quaternion.angvec2quat(math.radians(90), [0, 1, 0]))
p = map(lambda p0: quaternion.rotvecquat(p0, q), p)
for p0 in p:
self.surface.putpixel(int(r * p0[0] + x),
int(r * p0[1] + y), white)
except:
self.fittext(rectangle(0, .3, 1, .7), 'N/A')
def receive_message(self, msg):
name, value = msg
if self.m_notebook.GetSelection() == 0:
if name == 'imu.alignmentQ':
self.stAlignment.SetLabel(
str(round3(value)) + ' ' +
str(math.degrees(quaternion.angle(value))))
self.alignmentQ = value
elif name == 'imu.fusionQPose':
if not value:
return # no imu! show warning?
#lastaligned = quaternion.normalize(quaternion.multiply(self.fusionQPose, self.alignmentQ))
aligned = quaternion.normalize(
quaternion.multiply(value, self.alignmentQ))
value = aligned
if self.cCoords.GetSelection() == 1:
#self.boat_plot.Q = quaternion.multiply(self.boat_plot.Q, lastedaligned)
self.boat_plot.Q = quaternion.multiply(
self.boat_plot.Q, self.fusionQPose)
self.boat_plot.Q = quaternion.multiply(
self.boat_plot.Q, quaternion.conjugate(aligned))
elif self.cCoords.GetSelection() == 2:
ang = quaternion.toeuler(
self.fusionQPose)[2] - quaternion.toeuler(aligned)[2]
self.boat_plot.Q = quaternion.multiply(
self.boat_plot.Q,
quaternion.angvec2quat(ang, [0, 0, 1]))
self.fusionQPose = value
self.BoatPlot.Refresh()
elif name == 'imu.alignmentCounter':
self.gAlignment.SetValue(100 - value)
enable = value == 0
self.bLevel.Enable(enable)
elif name == 'imu.pitch':
self.stPitch.SetLabel(str(round3(value)))
elif name == 'imu.roll':
self.stRoll.SetLabel(str(round3(value)))
elif name == 'imu.heel':
self.stHeel.SetLabel(str(round3(value)))
elif name == 'imu.heading':
self.stHeading.SetLabel(str(round3(value)))
elif name == 'imu.heading_offset':
self.pypilot_heading_offset = value
self.heading_offset_timer.Start(1000, True)
elif self.m_notebook.GetSelection() == 1:
self.accel_calibration_plot.read_data(msg)
if name == 'imu.accel':
self.AccelCalibration.Refresh()
elif name == 'imu.accel.calibration':
self.stAccelCal.SetLabel(str(round3(value)))
elif name == 'imu.accel.calibration.age':
self.stAccelCalAge.SetLabel(str(value))
elif name == 'imu.accel.calibration.locked':
self.cbAccelCalibrationLocked.SetValue(value)
elif name == 'imu.accel.calibration.log':
self.tAccelCalibrationLog.WriteText(value + '\n')
elif self.m_notebook.GetSelection() == 2:
self.compass_calibration_plot.read_data(msg)
if name == 'imu.compass':
self.CompassCalibration.Refresh()
elif name == 'imu.compass.calibration':
self.stCompassCal.SetLabel(str(round3(value)))
elif name == 'imu.compass.calibration.age':
self.stCompassCalAge.SetLabel(str(value))
elif name == 'imu.compass.calibration.locked':
self.cbCompassCalibrationLocked.SetValue(value)
elif name == 'imu.compass.calibration.log':
self.tCompassCalibrationLog.WriteText(value + '\n')
elif self.m_notebook.GetSelection() == 3:
if name == 'rudder.angle':
self.UpdateLabel(self.stRudderAngle, str(round3(value)))
self.have_rudder = type(value) != type(bool)
elif name == 'rudder.offset':
self.UpdateLabel(self.stRudderOffset, str(round3(value)))
elif name == 'rudder.scale':
self.UpdateLabel(self.stRudderScale, (str(round3(value))))
elif name == 'rudder.nonlinearity':
self.UpdateLabel(self.stRudderNonlinearity, str(round3(value)))
elif name == 'rudder.range':
self.UpdatedSpin(self.sRudderRange, value)
elif name == 'servo.flags':
self.stServoFlags.SetLabel(value)
elif self.m_notebook.GetSelection() == 4:
if name in self.settings:
self.UpdatedSpin(self.settings[name], value)
def display(self):
width, height = self.dim
ar = float(width) / float(height)
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
fac = .05
glFrustum(-fac * ar, fac * ar, -fac, fac, .1, 15)
glMatrixMode(GL_MODELVIEW)
cal_new_bias = self.mag_cal_new_bias
cal_new_sphere = self.mag_cal_new_sphere
cal_sphere = self.mag_cal_sphere
glClearColor(0, 0, 0, 0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glPushMatrix()
s = self.userscale
glScalef(s, s, s)
TranslateAfter(0, 0, -1)
glPolygonMode(GL_FRONT_AND_BACK, self.mode)
if self.uncalibrated_view:
glPushMatrix()
glLineWidth(1)
down = [0, 0, 1]
if self.fusionQPose and self.alignmentQ:
q = quaternion.multiply(self.fusionQPose,
quaternion.conjugate(self.alignmentQ))
down = quaternion.rotvecquat(down, quaternion.conjugate(q))
#down = self.accel
glPushMatrix()
q = [1, 0, 0, 0]
if self.fusionQPose and self.alignmentQ:
q = quaternion.multiply(self.fusionQPose,
quaternion.conjugate(self.alignmentQ))
q = quaternion.normalize(q) # correct possible rounding errors
glRotatef(-math.degrees(quaternion.angle(q)), *q[1:])
if self.mag_fit_new_bias:
glColor3f(1, 0, 0)
self.mag_fit_new_bias.draw()
if self.mag_fit_new_sphere:
glColor3f(1, 0, 1)
self.mag_fit_new_sphere.draw()
if self.mag_fit_sphere:
glColor3f(0, 0, 1)
self.mag_fit_sphere.draw()
if self.mag_fit_cone:
glColor3f(1, 0, 0)
self.mag_fit_cone.draw()
glPopMatrix()
glTranslatef(-cal_sphere[0], -cal_sphere[1], -cal_sphere[2])
glPointSize(4)
glColor3f(1, .3, .3)
glBegin(GL_POINTS)
for i in xrange(max(len(self.recent_points) - recent_point_count, 0), \
len(self.recent_points)):
glVertex3fv(self.recent_points[i])
glEnd()
glPointSize(4)
glColor3f(0, 1, 0)
glBegin(GL_POINTS)
for i in xrange(len(self.points)):
glVertex3fv(self.points[i])
glEnd()
'''
glBegin(GL_LINE_STRIP)
for i in xrange(len(self.points)):
glVertex3fv(self.points[i])
glEnd()
'''
glColor3f(1, 1, 0)
glPointSize(6)
glBegin(GL_POINTS)
if self.sigmapoints:
for p in self.sigmapoints:
glVertex3fv(p[:3])
glEnd()
glColor3f(1, 1, 1)
glLineWidth(3.8)
glBegin(GL_LINES)
# glVertex3fv(cal[:3])
try:
'''
glColor3f(.8, 0, 0)
glVertex3fv(map(lambda x,y :-x*cal_new_bias[3]+y, down, cal_new_bias[:3]))
glVertex3fv(map(lambda x,y : x*cal_new_bias[3]+y, down, cal_new_bias[:3]))
glColor3f(.8, 0, .8)
glVertex3fv(map(lambda x,y :-x*cal_new_sphere[3]+y, down, cal_new_sphere[:3]))
glVertex3fv(map(lambda x,y : x*cal_new_sphere[3]+y, down, cal_new_sphere[:3]))
'''
glColor3f(.8, .8, .8)
glVertex3fv(
map(lambda x, y: -x * cal_sphere[3] + y, down,
cal_sphere[:3]))
glVertex3fv(
map(lambda x, y: x * cal_sphere[3] + y, down,
cal_sphere[:3]))
except:
print 'ERROR!!!!!!!!!!!!!!', self.accel, cal_sphere
glEnd()
glPopMatrix()
else: # calibrated view
glColor3f(0, 1, 1)
unit_sphere.draw()
glColor3f(1, 0, 1)
mag_plane_applied.draw()
def f_apply_sphere(beta, x):
return (x - beta[:3]) / beta[3]
cpoints = map(
lambda p: f_apply_sphere(numpy.array(cal), numpy.array(p)),
self.points)
glBegin(GL_LINE_STRIP)
for i in range(len(cpoints) - 10):
glVertex3fv(cpoints[i])
glColor3f(0, 1, 0)
for i in range(max(len(cpoints) - 10, 0), len(cpoints)):
glVertex3fv(cpoints[i])
glEnd()
glBegin(GL_LINE_STRIP)
glColor3f(1, 1, 0)
for i in range(len(self.apoints) / 10):
glVertex3fv(self.apoints[10 * i])
glEnd()
glPopMatrix()
