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