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.dsServoMaxCurrent.SetIncrement(.1) self.dsServoMaxCurrent.SetDigits(1) self.dsServoMaxCurrent.Bind(wx.EVT_SPINCTRLDOUBLE, self.onMaxCurrent) 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.signalk_heading_offset)), id=self.ID_HEADING_OFFSET) self.servo_timer = wx.Timer(self, self.ID_CALIBRATE_SERVO) self.Bind(wx.EVT_TIMER, self.calibrate_servo_timer, id=self.ID_CALIBRATE_SERVO) self.servoprocess = False self.alignmentQ = [1, 0, 0, 0] self.fusionQPose = [1, 0, 0, 0]
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.signalk_heading_offset)), id=self.ID_HEADING_OFFSET) self.have_rudder = False self.request_msg_timer = wx.Timer(self, self.ID_REQUEST_MSG) self.Bind(wx.EVT_TIMER, self.request_msg, id=self.ID_REQUEST_MSG) self.request_msg_timer.Start(250) self.fusionQPose = [1, 0, 0, 0] self.controltimes = {} self.settings = {}
def receive_message(self, msg): name, data = msg value = data['value'] self.compass_calibration_plot.read_data(msg) if name == 'imu.compass': self.CompassCalibration.Refresh() if name == 'imu.compass_calibration': self.stCompassCal.SetLabel(str(round3(value[0]))) self.stCompassCalDeviation.SetLabel('N/A') elif name == 'imu.compass_calibration_age': self.stCompassCalAge.SetLabel(str(value)) elif name == 'imu.compass_calibration_locked': self.cbCompassCalibrationLocked.SetValue(value) elif name == 'imu.alignmentQ': self.alignmentQ = value self.stAlignment.SetLabel( str(round3(value)) + ' ' + str(math.degrees(pypilot.quaternion.angle(self.alignmentQ)))) elif name == 'imu.fusionQPose': if self.cCoords.GetSelection() == 1: self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, self.fusionQPose) self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, pypilot.quaternion.conjugate(value)) elif self.cCoords.GetSelection() == 2: ang = pypilot.quaternion.toeuler( self.fusionQPose)[2] - pypilot.quaternion.toeuler(value)[2] self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, pypilot.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.signalk_heading_offset = value self.heading_offset_timer.Start(1000, True) elif name == 'servo.calibration': s = '' for name in value: s += name + ' = ' + str(value[name]) + '\n' self.stServoCalibration.SetLabel(s) self.SetSize(wx.Size(self.GetSize().x + 1, self.GetSize().y)) elif name == 'servo.calibration/console': self.stServoCalibrationConsole.SetLabel( self.stServoCalibrationConsole.GetLabel() + value) elif name == 'servo.max_current': self.dsServoMaxCurrent.SetValue(round3(value))
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.dsServoMaxCurrent.SetIncrement(.1) self.dsServoMaxCurrent.SetDigits(1) self.dsServoMaxCurrent.Bind(wx.EVT_SPINCTRLDOUBLE, self.onMaxCurrent) self.dsServoMaxControllerTemp.SetRange(45, 100) self.dsServoMaxControllerTemp.Bind(wx.EVT_SPINCTRL, self.onMaxControllerTemp) self.dsServoMaxMotorTemp.SetRange(30, 100) self.dsServoMaxMotorTemp.Bind(wx.EVT_SPINCTRL, self.onMaxMotorTemp) self.dsServoCurrentFactor.SetRange(.8, 1.2) self.dsServoCurrentFactor.SetIncrement(.0016) self.dsServoCurrentFactor.SetDigits(4) self.dsServoCurrentFactor.Bind(wx.EVT_SPINCTRLDOUBLE, self.onCurrentFactor) self.dsServoCurrentOffset.SetRange(-1.2, 1.2) self.dsServoCurrentOffset.SetIncrement(.01) self.dsServoCurrentOffset.SetDigits(2) self.dsServoCurrentOffset.Bind(wx.EVT_SPINCTRLDOUBLE, self.onCurrentOffset) self.dsServoVoltageFactor.SetRange(.8, 1.2) self.dsServoVoltageFactor.SetIncrement(.0016) self.dsServoVoltageFactor.SetDigits(4) self.dsServoVoltageFactor.Bind(wx.EVT_SPINCTRLDOUBLE, self.onVoltageFactor) self.dsServoVoltageOffset.SetRange(-1.2, 1.2) self.dsServoVoltageOffset.SetIncrement(.01) self.dsServoVoltageOffset.SetDigits(2) self.dsServoVoltageOffset.Bind(wx.EVT_SPINCTRLDOUBLE, self.onVoltageOffset) self.dsServoMaxSlewSpeed.SetRange(0, 100) self.dsServoMaxSlewSpeed.Bind(wx.EVT_SPINCTRL, self.onMaxSlewSpeed) self.dsServoMaxSlewSlow.SetRange(0, 100) self.dsServoMaxSlewSlow.Bind(wx.EVT_SPINCTRL, self.onMaxSlewSlow) self.dsServoGain.SetIncrement(.1) self.dsServoGain.SetDigits(1) self.dsServoGain.Bind(wx.EVT_SPINCTRLDOUBLE, self.onServoGain) 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.signalk_heading_offset)), id=self.ID_HEADING_OFFSET) self.servo_timer = wx.Timer(self, self.ID_CALIBRATE_SERVO) self.Bind(wx.EVT_TIMER, self.calibrate_servo_timer, id=self.ID_CALIBRATE_SERVO) self.request_msg_timer = wx.Timer(self, self.ID_REQUEST_MSG) self.Bind(wx.EVT_TIMER, self.request_msg, id=self.ID_REQUEST_MSG) self.request_msg_timer.Start(200) self.servoprocess = False self.alignmentQ = [1, 0, 0, 0] self.fusionQPose = [1, 0, 0, 0]
def receive_message(self, msg): name, data = msg value = data['value'] 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) 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[0]))) elif name == 'imu.compass.calibration.age': self.stCompassCalAge.SetLabel(str(value)) elif name == 'imu.compass.calibration.locked': self.cbCompassCalibrationLocked.SetValue(value) elif name == 'imu.alignmentQ': self.alignmentQ = value self.stAlignment.SetLabel( str(round3(value)) + ' ' + str(math.degrees(pypilot.quaternion.angle(self.alignmentQ)))) elif name == 'imu.fusionQPose': if self.cCoords.GetSelection() == 1: self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, self.fusionQPose) self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, pypilot.quaternion.conjugate(value)) elif self.cCoords.GetSelection() == 2: ang = pypilot.quaternion.toeuler( self.fusionQPose)[2] - pypilot.quaternion.toeuler(value)[2] self.boat_plot.Q = pypilot.quaternion.multiply( self.boat_plot.Q, pypilot.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.signalk_heading_offset = value self.heading_offset_timer.Start(1000, True) elif name == 'servo.rudder': self.stRudderAngle.SetLabel(str(round3(value))) self.rudder = value elif name == 'servo.flags': self.stServoFlags.SetLabel(value) elif name == 'servo.rudder.offset': self.stRudderOffset.SetLabel(str(round3(value))) self.rudder_offset = value elif name == 'servo.rudder.scale': self.rudder_scale = value elif name == 'servo.rudder.range': self.sRudderRange.SetValue(value) self.rudder_range = value elif name == 'servo.calibration': s = '' for name in value: s += name + ' = ' + str(value[name]) + '\n' self.stServoCalibration.SetLabel(s) self.SetSize(wx.Size(self.GetSize().x + 1, self.GetSize().y)) elif name == 'servo.max_current': self.dsServoMaxCurrent.SetValue(round3(value)) elif name == 'servo.max_controller_temp': self.dsServoMaxControllerTemp.SetValue(value) elif name == 'servo.max_motor_temp': self.dsServoMaxMotorTemp.SetValue(value) elif name == 'servo.current.factor': self.dsServoCurrentFactor.SetValue(round(value, 4)) elif name == 'servo.current.offset': self.dsServoCurrentOffset.SetValue(value) elif name == 'servo.voltage.factor': self.dsServoVoltageFactor.SetValue(round(value, 4)) elif name == 'servo.voltage.offset': self.dsServoVoltageOffset.SetValue(value) elif name == 'servo.max_slew_speed': self.dsServoMaxSlewSpeed.SetValue(value) elif name == 'servo.max_slew_slow': self.dsServoMaxSlewSlow.SetValue(value) elif name == 'servo.voltage': self.m_stServoVoltage.SetLabel(str(round3(value))) elif name == 'servo.current': self.m_stServoCurrent.SetLabel(str(round3(value)))
def receive_message(self, msg): name, data = msg value = data['value'] if self.m_notebook.GetSelection() == 0: if name == 'imu.alignmentQ': self.stAlignment.SetLabel(str(round3(value)) + ' ' + str(math.degrees(pypilot.quaternion.angle(value)))) elif name == 'imu.fusionQPose': if not value: return # no imu! show warning? if self.cCoords.GetSelection() == 1: self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, self.fusionQPose) self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, pypilot.quaternion.conjugate(value)) elif self.cCoords.GetSelection() == 2: ang = pypilot.quaternion.toeuler(self.fusionQPose)[2] - pypilot.quaternion.toeuler(value)[2] self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, pypilot.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.signalk_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 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[0]))) elif name == 'imu.compass.calibration.age': self.stCompassCalAge.SetLabel(str(value)) elif name == 'imu.compass.calibration.locked': self.cbCompassCalibrationLocked.SetValue(value) 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 self.m_notebook.GetSelection() == 4: for n in self.settings: if name == n: self.UpdatedSpin(self.settings[name], value) break