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 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 glRotateQ(q):
try:
glRotatef(
quaternion.angle(q) * 180 / math.pi, q[1], q[2], q[3])
except:
pass
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()
