def __init__(self):
super(CalibrationDialog, self).__init__(None)
self.host = ''
if len(sys.argv) > 1:
self.host = sys.argv[1]
self.client = False
self.accel_calibration_plot = calibration_plot.AccelCalibrationPlot()
self.accel_calibration_glContext = wx.glcanvas.GLContext(
self.AccelCalibration)
self.compass_calibration_plot = calibration_plot.CompassCalibrationPlot(
)
self.compass_calibration_glContext = wx.glcanvas.GLContext(
self.CompassCalibration)
self.boat_plot = boatplot.BoatPlot()
self.boat_plot_glContext = wx.glcanvas.GLContext(self.BoatPlot)
self.lastmouse = False
self.alignment_count = 0
self.timer = wx.Timer(self, self.ID_MESSAGES)
self.timer.Start(50)
self.Bind(wx.EVT_TIMER, self.receive_messages, id=self.ID_MESSAGES)
self.heading_offset_timer = wx.Timer(self, self.ID_HEADING_OFFSET)
self.Bind(wx.EVT_TIMER,
lambda e: self.sHeadingOffset.SetValue(
round3(self.pypilot_heading_offset)),
id=self.ID_HEADING_OFFSET)
self.have_rudder = False
self.fusionQPose = [1, 0, 0, 0]
self.alignmentQ = [1, 0, 0, 0]
self.controltimes = {}
self.client = pypilotClient(self.host)
# clear out plots
self.accel_calibration_plot.points = []
self.compass_calibration_plot.points = []
self.settings = {}
self.set_watches()
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)
