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 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()