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() if not self.fusionQPose: return 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 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 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()