Ejemplo n.º 1
0
    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
Ejemplo n.º 2
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)
Ejemplo n.º 3
0
    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()