示例#1
0
 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()
示例#2
0
    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()
示例#3
0
    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')
示例#4
0
    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')
示例#5
0
    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)