class RFWebConfig(Namespace): def __init__(self, name): super(Namespace, self).__init__(name) socketio.start_background_task(target=self.background_thread) self.client = False self.last_values = {} self.last_code = False self.last_code_time = time.time() self.servo_timeout = time.time() + 1 self.actions = [RFActionEngage(self), RFActionSignalK(self, 'disengage', 'ap.enabled', False), RFActionHeading(self, 1), RFActionHeading(self, -1), RFActionHeading(self, 2), RFActionHeading(self, -2), RFActionHeading(self, 10), RFActionHeading(self, -10), RFActionSignalK(self, 'compassmode', 'ap.mode', 'compass'), RFActionSignalK(self, 'gpsmode', 'ap.mode', 'gps'), RFActionSignalK(self, 'windmode', 'ap.mode', 'wind'), RFActionTack(self, 'tackport', 'port'), RFActionTack(self, 'tackstarboard', 'starboard')] self.read_config() def read_config(self): config = {} try: file = open(config_path) config = json.loads(file.read()) file.close() except: pass for action in self.actions: if action.name in config: action.codes = config[action.name] def write_config(self): config = {} for action in self.actions: config[action.name] = action.codes try: file = open(config_path, 'w') file.write(json.dumps(config) + '\n') except IOError: print('failed to save config file:', self.configfilename) def on_ping(self): emit('pong') def on_codes(self, command): if not self.last_code: return # remove this code from any actions for action in self.actions: while self.last_code in action.codes: action.codes.remove(self.last_code) # add the last code to the action for action in self.actions: if command == action.name: action.codes.append(self.last_code) break self.emit_codes() self.write_config() def emit_codes(self): for action in self.actions: codes = {'name': action.name, 'codes': map(lambda code : "%X" % code, action.codes)} socketio.emit('action_codes', codes) def on_connect(self): #self.clients[request.sid] = Connection() if self.client: socketio.emit('pypilot', 'Connected') self.emit_codes() print('Client connected', request.sid) def on_disconnect(self): print('Client disconnected', request.sid) def apply_code(self, code): socketio.emit('rfcode', "%X" % code) self.last_code = code self.last_code_time = time.time() for action in self.actions: if code in action.codes: socketio.emit('action', action.name) action.trigger() def background_thread(self): try: spi = spidev.SpiDev() spi.open(0, 1) spi.max_speed_hz=5000 except Exception as e: print('failed to open spi device', e) exit(1) watchlist = ['ap.enabled', 'ap.heading_command'] for name in watchlist: self.last_values[name] = 0 def on_con(client): for name in watchlist: client.watch(name) socketio.emit('pypilot', 'Connected') self.client = False dt = 0 lastpollheading = time.time() while True: if self.client: try: while True: msg = self.client.receive_single() if not msg: break name, value = msg self.last_values[name] = value['value'] except Exception as e: socketio.emit('pypilot', 'Disconnected' + str(e)) self.client = False if not self.client: socketio.sleep(3) try: self.client = SignalKClient(on_con, host) print('connected', host) except Exception as e: print('failed to connect', e) t = time.time() dtc = t - self.last_code_time if dtc > 8 and self.last_code: self.last_code = False socketio.emit('rfcode', 'N/A') socketio.emit('action', '') # poll heading once per second if not enabled dtp = t - lastpollheading if self.client and dtp > 1 and not self.last_values['ap.enabled']: self.client.get('ap.heading') lastpollheading = t # timeout manual move if self.servo_timeout: dtt = t - self.servo_timeout if dtt > 0: self.client.set('servo.command', 0) #stop self.servo_timeout = 0 x = spi.xfer([0, 0, 0, 0]) if not any(x): socketio.sleep(dt) dt = min(.1, dt+.001) continue dt = 0 for i in range(4): if not x[0] and x[1] and x[2] and x[3]: # ok code = (x[1]*256 + x[2])*256 + x[3] if self.last_code != code or dtc > .6: self.apply_code(code) break x = x[1:] + [spi.xfer([0])[0]] spi.close()
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase): ID_MESSAGES = 1000 ID_CALIBRATE_SERVO = 1001 ID_HEADING_OFFSET = 1002 def __init__(self): super(CalibrationDialog, self).__init__(None) self.host = '' if len(sys.argv) > 1: self.host = sys.argv[1] self.client = False self.compass_calibration_plot = compass_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 on_con(self, client): watchlist = ['imu.compass_calibration', 'imu.compass_calibration_age', \ 'imu.compass', 'imu.compass_calibration_sigmapoints', \ 'imu.compass_calibration_locked', \ 'imu.accel', 'imu.fusionQPose', 'imu.alignmentCounter', \ 'imu.heading', \ 'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel', \ 'imu.heading_offset', 'servo.calibration', 'servo.max_current'] for name in watchlist: client.watch(name) def receive_messages(self, event): if not self.client: try: self.client = SignalKClient(self.on_con, self.host, autoreconnect=False) except socket.error: self.timer.Start(5000) return try: msg = self.client.receive_single() while msg: self.receive_message(msg) msg = self.client.receive_single() self.timer.Start(50) except ConnectionLost: self.client = False 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 servo_console(self, text): self.stServoCalibrationConsole.SetLabel( self.stServoCalibrationConsole.GetLabel() + text + '\n') def calibrate_servo_timer(self, event): if not self.servoprocess: return self.servoprocess.poll() print 'servotimer', self.servoprocess.returncode, self.servoprocess.returncode == None if self.servoprocess.returncode == None: self.servoprocess.communicate() line = self.servoprocess.stdout.readline() print 'line', line if line: self.servo_console(line) self.servo_timer.Start(150, True) else: self.bCalibrateServo.Enable() if self.servoprocess.returncode == 0: file = open('servo_calibration') calibration = json.loads(file.readline()) self.client.set('servo.calibration', calibration) self.servo_console('calibration sent.') else: self.servo_console('calibration failed.') self.servoprocess = False def onKeyPressCompass(self, event): signalk.scope_wx.wxglutkeypress(event, self.compass_calibration_plot.special, \ self.compass_calibration_plot.key) def onClearCompass(self, event): self.compass_calibration_plot.points = [] def onCompassCalibrationLocked(self, event): self.client.set('imu.compass_calibration_locked', self.cbCompassCalibrationLocked.GetValue()) def onMouseEventsCompass(self, event): self.CompassCalibration.SetFocus() pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): compass_calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \ pos[1] - self.lastmouse[1]) self.CompassCalibration.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: self.CompassCalibration.Refresh() while rotation > 0: self.compass_calibration_plot.userscale /= .9 rotation -= 1 while rotation < 0: self.compass_calibration_plot.userscale *= .9 rotation += 1 def onPaintGLCompass(self, event): wx.PaintDC(self.CompassCalibration) self.CompassCalibration.SetCurrent(self.compass_calibration_glContext) self.compass_calibration_plot.display() self.CompassCalibration.SwapBuffers() def onSizeGLCompass(self, event): self.compass_calibration_plot.reshape(event.GetSize().x, event.GetSize().y) def StartAlignment(self): self.client.set('imu.alignmentCounter', 100) def onResetAlignment(self, event): self.client.set('imu.alignmentQ', [1, 0, 0, 0]) def onLevel(self, event): self.StartAlignment() def onIMUHeadingOffset(self, event): self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue()) self.heading_offset_timer.Stop() def onKeyPressBoatPlot(self, event): self.BoatPlot.SetFocus() k = '%c' % (event.GetKeyCode() & 255) if not event.GetModifiers() & wx.MOD_SHIFT: k = k.lower() self.BoatPlot.Refresh() def onMouseEventsBoatPlot(self, event): self.BoatPlot.SetFocus() pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1] q = pypilot.quaternion.angvec2quat( (dx**2 + dy**2)**.4 / 180 * math.pi, [dy, dx, 0]) self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q) self.BoatPlot.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: self.BoatPlot.Refresh() while rotation > 0: self.boat_plot.Scale /= .9 rotation -= 1 while rotation < 0: self.boat_plot.Scale *= .9 rotation += 1 def onPaintGLBoatPlot(self, event): wx.PaintDC(self.CompassCalibration) self.BoatPlot.SetCurrent(self.boat_plot_glContext) # stupid hack self.boat_plot.reshape(self.BoatPlot.GetSize().x, self.BoatPlot.GetSize().y) self.boat_plot.display(self.fusionQPose) self.BoatPlot.SwapBuffers() def onSizeGLBoatPlot(self, event): self.boat_plot.reshape(event.GetSize().x, event.GetSize().y) self.BoatPlot.Refresh() def onTextureCompass(self, event): self.boat_plot.texture_compass = event.IsChecked() self.BoatPlot.Refresh() def onIMUScope(self, event): host, port = self.client.host_port args = [ 'python', os.path.abspath(os.path.dirname(__file__)) + '/../signalk/scope_wx.py', host + ':' + str(port), 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading' ] subprocess.Popen(args) def onCalibrateServo(self, event): try: self.servoprocess = subprocess.Popen( ['python', 'servo_calibration.py', sys.argv[1]], stdout=subprocess.PIPE) self.servo_console('executed servo_calibration.py...') self.servo_timer.Start(150, True) self.bCalibrateServo.Disable() except OSError: self.servo_console('Failed to execute servo_calibration.py.\n') def onMaxCurrent(self, event): self.client.set('servo.max_current', event.GetValue())
class autogain(object): def __init__(self): self.search = [ \ #{'name': 'ap.I', 'min': 0, 'max': .006, 'step': .003}, #{'name': 'ap.P2', 'min': 0, 'max': .006, 'step': .006}, {'name': 'ap.P', 'min': .002, 'max': .006, 'step': .001}, {'name': 'ap.D', 'min': .06, 'max': .12, 'step': .01}] self.variables = ['ap.heading_error', 'servo.Watts', 'servo.current', 'gps.speed'] self.settle_period = 2 self.period = 5 self.watchlist = ['ap.enabled'] for var in self.search: self.watchlist.append(var['name']) for var in self.variables: self.watchlist.append(var) def on_con(client): for name in self.watchlist: client.watch(name) print 'connecting to server...' host = False if len(sys.argv) > 1: host = sys.argv[1] while True: try: self.client = SignalKClient(on_con, host, autoreconnect=True) break except: time.sleep(2) print 'connected' def read_messages(self, log): msgs = self.client.receive() for name in msgs: data = msgs[name] value = data['value'] for var in self.search: if name == var: name = name[3:] if abs(value - self.gains[name].value) > 1e-8: print 'external program adjusting search variable!!, abrort', name, value exit(0) if log: for var in self.variables: if name == var: self.total[name]['total'] += value self.total[name]['count'] += 1 if name == 'ap.enabled' and not value: #print 'autopilot disabled!!' #exit(0) pass def set(self, name, val): print 'setting', name, 'to', val self.searchval[name] = val self.client.set(name, val) def log(self): print 'logging for', self.searchval t0 = time.time() self.total = {} for var in self.variables: self.total[var] = {'total': 0, 'count': 0} while time.time() - t0 < self.period: self.read_messages(time.time() - t0 > self.settle_period) time.sleep(.05) for var in self.variables: if not var in self.results: self.results[var] = [] count = self.total[var]['count'] if count: self.results[var].append((self.searchval.copy(), self.total[var]['total'] / count)) else: print 'warning, no results for', var def run_search(self, search): if search: s = search[0] for val in frange(s['min'], s['max'], s['step']): self.set(s['name'], val) self.run_search(search[1:]) else: self.log() def result_range(self, results, name): r = [] for result in results: vars, val = result r.append(vars[name]) return unique(sorted(r)) def result_value(self, results, vals): values = [] for result in results: vars, val = result if vars == vals: values.append(val) if len(values) == 1: return '%.3f' % values[0] return values def print_results(self, results, search, vals): l = len(search) if l < 2: print 'error, need at least 2 search variables' exit(1) s = search[0] if l > 2: for val in self.result_range(results, s['name']): print s['name'], '=', val vals[s['name']] = val self.print_results(results, search[1:], vals) elif l == 2: t = search[1] print s['name'], '/', t['name'] line = '\t' s_range = self.result_range(results, s['name']) for val0 in s_range: line += '%.3f\t' % val0 print line for val1 in self.result_range(results, t['name']): line = '%.3f\t' % val1 vals[t['name']] = val1 for val0 in s_range: vals[s['name']] = val0 line += str(self.result_value(results, vals)) + '\t' print line print '' def run(self): self.searchval = {} self.results = {} self.run_search(self.search) for var in self.variables: print 'Results for', var self.print_results(self.results[var], self.search, {}) print ''
class MainFrame(wx.Frame): def __init__(self): wx.Frame.__init__(self, None, title="signalk client", size=(1000, 600)) self.value_list = [] self.client = SignalKClientFromArgs(sys.argv, True, self.on_con) self.host_port = self.client.host_port self.client.autoreconnect = False ssizer = wx.FlexGridSizer(0, 1, 0, 0) ssizer.AddGrowableRow(0) ssizer.AddGrowableCol(0) ssizer.SetFlexibleDirection(wx.BOTH) ssizer.SetNonFlexibleGrowMode(wx.FLEX_GROWMODE_SPECIFIED) self.scrolledWindow = wx.ScrolledWindow(self, wx.ID_ANY, wx.DefaultPosition, wx.DefaultSize, wx.HSCROLL | wx.VSCROLL) self.scrolledWindow.SetScrollRate(5, 5) sizer = wx.FlexGridSizer(0, 3, 0, 0) sizer.AddGrowableCol(2) sizer.SetFlexibleDirection(wx.BOTH) sizer.SetNonFlexibleGrowMode(wx.FLEX_GROWMODE_SPECIFIED) self.values = {} self.controls = {} self.sliderrange = {} self.value_list = self.client.list_values() self.on_con(self.client) for name in sorted(self.value_list): sizer.Add(wx.StaticText(self.scrolledWindow, wx.ID_ANY, name), 0, wx.ALL, 5) self.values[name] = wx.StaticText(self.scrolledWindow, wx.ID_ANY) sizer.Add(self.values[name], 0, wx.ALL, 5) t = self.value_list[name]['type'] if t == 'Property': tb = wx.TextCtrl(self.scrolledWindow, wx.ID_ANY) sizer.Add(tb) self.controls[name] = tb elif t == 'BooleanProperty': def proc(): # encapsulate to fix scope cb = wx.CheckBox(self.scrolledWindow, wx.ID_ANY, '') sizer.Add(cb, 0, wx.EXPAND) self.controls[name] = cb cbname = name def oncheck(event): self.client.set(cbname, cb.GetValue()) cb.Bind(wx.EVT_CHECKBOX, oncheck) proc() elif t == 'RangeProperty' or t == 'RangeSetting': useSlider = True def proc(): r = self.value_list[name]['min'], self.value_list[name][ 'max'] if useSlider: s = wx.Slider(self.scrolledWindow) s.SetRange(0, 1000) else: s = wx.SpinCtrlDouble(self.scrolledWindow) s.SetRange(r[0], r[1]) s.SetIncrement(min(1, (r[1] - r[0]) / 100.0)) s.SetDigits(-math.log(s.GetIncrement()) / math.log(10) + 1) sizer.Add(s, 0, wx.EXPAND) self.controls[name] = s sname = name def onspin(event): if useSlider: v = s.GetValue() / 1000.0 * (r[1] - r[0]) + r[0] self.client.set(sname, v) else: self.client.set(sname, s.GetValue()) if useSlider: s.Bind(wx.EVT_SLIDER, onspin) self.sliderrange[name] = r else: s.Bind(wx.EVT_SPINCTRLDOUBLE, onspin) proc() elif t == 'EnumProperty': def proc(): c = wx.Choice(self.scrolledWindow, wx.ID_ANY) for choice in self.value_list[name]['choices']: c.Append(str(choice)) sizer.Add(c, 0, wx.EXPAND) self.controls[name] = c cname = name def onchoice(event): self.client.set(cname, str(c.GetStringSelection())) c.Bind(wx.EVT_CHOICE, onchoice) proc() elif t == 'ResettableValue': def proc(): b = wx.Button(self.scrolledWindow, wx.ID_ANY, 'Reset') sizer.Add(b, 0, wx.EXPAND) bname = name def onclick(event): self.client.set(bname, 0) b.Bind(wx.EVT_BUTTON, onclick) proc() else: sizer.Add(wx.StaticText(self.scrolledWindow, wx.ID_ANY, '')) self.scrolledWindow.SetSizer(sizer) self.scrolledWindow.Layout() sizer.Fit(self.scrolledWindow) ssizer.Add(self.scrolledWindow, 1, wx.EXPAND | wx.ALL, 5) bsizer = wx.FlexGridSizer(1, 0, 0, 0) self.bRefresh = wx.Button(self, wx.ID_ANY, 'Refresh') self.bRefresh.Bind(wx.EVT_BUTTON, self.Refresh) bsizer.Add(self.bRefresh) self.bScope = wx.Button(self, wx.ID_ANY, 'Scope') self.bScope.Bind( wx.EVT_BUTTON, lambda event: subprocess.Popen([ 'python', os.path.abspath(os.path.dirname(__file__)) + '/' + 'scope_wx.py' ] + sys.argv[1:])) bsizer.Add(self.bScope) self.bClose = wx.Button(self, wx.ID_ANY, 'Close') self.bClose.Bind(wx.EVT_BUTTON, exit) bsizer.Add(self.bClose) ssizer.Add(bsizer, 1, wx.EXPAND) self.SetSizer(ssizer) self.Layout() self.timer = wx.Timer(self, wx.ID_ANY) self.timer.Start(500) self.Bind(wx.EVT_TIMER, self.receive_messages, id=wx.ID_ANY) self.Refresh() def Refresh(self): for name in self.value_list: self.client.get(name) def on_con(self, client): self.SetTitle("signalk client - Connected") for name in sorted(self.value_list): t = self.value_list[name]['type'] if t != 'SensorValue': client.watch(name) else: client.get(name) def receive_messages(self, event): if not self.client: try: host, port = self.host_port self.client = SignalKClient(self.on_con, host, port, autoreconnect=False) self.timer.Start(100) except socket.error: self.timer.Start(1000) return while True: result = False try: result = self.client.receive() except ConnectionLost: self.SetTitle("signalk client - Disconnected") self.client = False return except: pass if not result: break for name in result: if not 'value' in result[name]: print('no value', result) raise 'no value' value = round3(result[name]['value']) strvalue = str(value) if len(strvalue) > 50: strvalue = strvalue[:47] + '...' self.values[name].SetLabel(strvalue) if name in self.controls: try: if str(type(self.controls[name]) ) == "<class 'wx._controls.Choice'>": if not self.controls[name].SetStringSelection( value): print( 'warning, invalid choice value specified') elif str(type(self.controls[name]) ) == "<class 'wx._controls.Slider'>": r = self.sliderrange[name] self.controls[name].SetValue( float(value - r[0]) / (r[1] - r[0]) * 1000) else: self.controls[name].SetValue(value) except: self.controls[name].SetValue(str(value)) size = self.GetSize() self.Fit() self.SetSize(size)
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase): ID_MESSAGES = 1000 ID_CALIBRATE_SERVO = 1001 ID_HEADING_OFFSET = 1002 ID_REQUEST_MSG = 1003 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 set_watches(self, client): if not client: return watchlist = [ ['imu.fusionQPose', 'imu.alignmentCounter', 'imu.heading', 'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading_offset'], ['imu.accel.calibration', 'imu.accel.calibration.age', 'imu.accel', 'imu.accel.calibration.sigmapoints', 'imu.accel.calibration.locked'], ['imu.fusionQPose', 'imu.compass.calibration', 'imu.compass.calibration.age', 'imu.compass', 'imu.compass.calibration.sigmapoints', 'imu.compass.calibration.locked'], ['rudder.offset', 'rudder.scale', 'rudder.nonlinearity', 'rudder.range', 'rudder.calibration_state']] watchlist.append(list(self.settings)) pageindex = 0 for pagelist in watchlist: for name in pagelist: client.watch(name, False) pageindex = self.m_notebook.GetSelection() for name in watchlist[pageindex]: client.watch(name) def on_con(self, client): values = client.list_values() if not self.settings: fgSettings = wx.FlexGridSizer( 0, 3, 0, 0 ) fgSettings.AddGrowableCol( 1 ) fgSettings.SetFlexibleDirection( wx.BOTH ) fgSettings.SetNonFlexibleGrowMode( wx.FLEX_GROWMODE_SPECIFIED ) self.m_pSettings.SetSizer( fgSettings ) self.m_pSettings.Layout() fgSettings.Fit( self.m_pSettings ) lvalues = list(values) lvalues.sort() for name in lvalues: if 'units' in values[name]: v = values[name] def proc(): s = wx.SpinCtrlDouble(self.m_pSettings, wx.ID_ANY) s.SetRange(v['min'], v['max']) s.SetIncrement(min(1, (v['max'] - v['min']) / 100.0)) s.SetDigits(-math.log(s.GetIncrement()) / math.log(10) + 1) self.settings[name] = s fgSettings.Add(wx.StaticText(self.m_pSettings, wx.ID_ANY, name), 0, wx.ALL, 5) fgSettings.Add(s, 0, wx.ALL | wx.EXPAND, 5) fgSettings.Add(wx.StaticText(self.m_pSettings, wx.ID_ANY, v['units']), 0, wx.ALL, 5) sname = name def onspin(event): self.client.set(sname, s.GetValue()) s.Bind( wx.EVT_SPINCTRLDOUBLE, onspin ) proc() fgSettings.Add( ( 0, 0), 1, wx.EXPAND, 5 ) fgSettings.Add( ( 0, 0), 1, wx.EXPAND, 5 ) b = wx.Button( self.m_pSettings, wx.ID_OK ) fgSettings.Add ( b, 1, wx.ALIGN_RIGHT, 5) self.set_watches(client) def receive_messages(self, event): if not self.client: try: self.client = SignalKClient(self.on_con, self.host, autoreconnect=False) except socket.error: self.timer.Start(5000) return try: msg = self.client.receive_single() while msg: self.receive_message(msg) msg = self.client.receive_single() self.timer.Start(50) except ConnectionLost: self.client = False def request_msg(self, event): if not self.client: return page_gets = [[], [], [], ['rudder.angle'], []] i = self.m_notebook.GetSelection() for name in page_gets[i]: self.client.get(name) def UpdateControl(self, control, update): t = time.time() if not control in self.controltimes or t - self.controltimes[control] > .5: update() self.controltimes[control] = t def UpdateLabel(self, label, value): self.UpdateControl(label, lambda : label.SetLabel(str(value))) def UpdatedSpin(self, dspin, value): self.UpdateControl(dspin, lambda : dspin.SetValue(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 def servo_console(self, text): self.stServoCalibrationConsole.SetLabel(self.stServoCalibrationConsole.GetLabel() + text + '\n') def PageChanged( self, event ): self.set_watches(self.client) def onKeyPressAccel( self, event ): self.onKeyPress(event, self.compass_calibration_plot) def onKeyPressCompass( self, event ): self.onKeyPress(event, self.compass_calibration_plot) def onKeyPress( self, event, plot ): signalk.scope_wx.wxglutkeypress(event, plot.special, plot.key) def onClearAccel( self, event ): self.accel_calibration_plot.points = [] def onClearCompass( self, event ): self.compass_calibration_plot.points = [] def onAccelCalibrationLocked( self, event ): self.client.set('imu.accel.calibration.locked', self.cbAccelCalibrationLocked.GetValue()) def onCompassCalibrationLocked( self, event ): self.client.set('imu.compass.calibration.locked', self.cbCompassCalibrationLocked.GetValue()) def onCalibrationLocked( self, sensor, ctrl ): self.client.set('imu.'+sensor+'.calibration.locked', self.ctrl.GetValue()) def onMouseEventsAccel( self, event ): self.AccelCalibration.SetFocus() self.onMouseEvents( event, self.AccelCalibration, self.accel_calibration_plot ) def onMouseEventsCompass( self, event ): self.CompassCalibration.SetFocus() self.onMouseEvents( event, self.CompassCalibration, self.compass_calibration_plot ) def onMouseEvents( self, event, canvas, plot ): pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \ pos[1] - self.lastmouse[1]) canvas.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: canvas.Refresh() while rotation > 0: plot.userscale /= .9 rotation -= 1 while rotation < 0: plot.userscale *= .9 rotation += 1 def onPaintGLAccel( self, event ): self.onPaintGL( self.AccelCalibration, self.accel_calibration_plot, self.accel_calibration_glContext ) def onPaintGLCompass( self, event ): self.onPaintGL( self.CompassCalibration, self.compass_calibration_plot, self.compass_calibration_glContext ) def onPaintGL( self, canvas, plot, context ): wx.PaintDC( canvas ) canvas.SetCurrent(context) plot.display() canvas.SwapBuffers() def onSizeGLAccel( self, event ): self.accel_calibration_plot.reshape(event.GetSize().x, event.GetSize().y) def onSizeGLCompass( self, event ): self.compass_calibration_plot.reshape(event.GetSize().x, event.GetSize().y) def onResetAlignment(self, event): self.client.set('imu.alignmentQ', [1, 0, 0, 0]) def onLevel( self, event ): self.client.set('imu.alignmentCounter', 100) def onIMUHeadingOffset( self, event ): self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue()) self.heading_offset_timer.Stop() def onKeyPressBoatPlot( self, event ): self.BoatPlot.SetFocus() k = '%c' % (event.GetKeyCode()&255) if not event.GetModifiers() & wx.MOD_SHIFT: k = k.lower() self.BoatPlot.Refresh() def onMouseEventsBoatPlot( self, event ): self.BoatPlot.SetFocus() pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1] q = pypilot.quaternion.angvec2quat((dx**2 + dy**2)**.4/180*math.pi, [dy, dx, 0]) self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q) self.BoatPlot.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: self.BoatPlot.Refresh() while rotation > 0: self.boat_plot.Scale /= .9 rotation -= 1 while rotation < 0: self.boat_plot.Scale *= .9 rotation += 1 def onPaintGLBoatPlot( self, event ): wx.PaintDC( self.BoatPlot ) self.BoatPlot.SetCurrent(self.boat_plot_glContext) # stupid hack self.boat_plot.reshape(self.BoatPlot.GetSize().x, self.BoatPlot.GetSize().y) self.boat_plot.display(self.fusionQPose) self.BoatPlot.SwapBuffers() def onSizeGLBoatPlot( self, event ): self.boat_plot.reshape(event.GetSize().x, event.GetSize().y) self.BoatPlot.Refresh() def onTextureCompass( self, event ): self.boat_plot.texture_compass = event.IsChecked() self.BoatPlot.Refresh() def onIMUScope( self, event ): host, port = self.client.host_port args = ['python', os.path.abspath(os.path.dirname(__file__)) + '/../signalk/scope_wx.py', host + ':' + str(port), 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading'] subprocess.Popen(args) def onRudderResetCalibration( self, event ): self.client.set('rudder.calibration_state', 'reset') def onRudderCentered( self, event ): self.client.set('rudder.calibration_state', 'centered') def onRudderStarboardRange( self, event ): self.client.set('rudder.calibration_state', 'starboard range') def onRudderPortRange( self, event ): self.client.set('rudder.calibration_state', 'port range') def onRudderRange( self, event ): self.client.set('rudder.range', event.GetValue())
class LCDClient(): def __init__(self, screen): # w, h, self.bw = 44, 84, 1 # w, h, self.bw = 64, 128, 1 # w, h, self.bw = 320, 480, 0 self.config = {} self.configfilename = os.getenv('HOME') + '/.pypilot/lcdclient.conf' self.config['contrast'] = 60 self.config['invert'] = False self.config['flip'] = False self.config['language'] = 'en' self.config['bigstep'] = 10 self.config['smallstep'] = 1 print 'loading load config file:', self.configfilename try: file = open(self.configfilename) config = json.loads(file.readline()) for name in config: self.config[name] = config[name] except: print 'failed to load config file:', self.configfilename if screen: w, h = screen.width, screen.height mul = int(math.ceil(w / 48.0)) self.bw = 1 if w < 256 else False width = min(w, 48*mul) self.surface = ugfx.surface(width, width*h/w, screen.bypp, None) self.frameperiod = .25 # 4 frames a second possible else: self.surface = None self.frameperiod = 1 self.set_language(self.config['language']) self.range_edit = False self.modes = {'compass': self.have_compass, 'gps': self.have_gps, 'wind': self.have_wind, 'true wind': self.have_true_wind}; self.modes_list = ['compass', 'gps', 'wind', 'true wind'] # in order self.initial_gets = gains + ['servo.min_speed', 'servo.max_speed', 'servo.max_current', 'servo.period', 'imu.alignmentCounter'] self.have_select = False self.create_mainmenu() self.longsleep = 30 self.display_page = self.display_connecting self.connecting_dots = 0 self.client = False self.keystate = {} self.keypad = [False, False, False, False, False, False, False, False] self.keypadup = list(self.keypad) self.blink = black, white self.control = False self.wifi = False self.overcurrent_time = 0 if orangepi: self.pins = [11, 16, 13, 15, 12] else: self.pins = [17, 23, 27, 22, 18] if GPIO: if orangepi: for pin in self.pins: cmd = 'gpio -1 mode ' + str(pin) + ' up' os.system(cmd) GPIO.setmode(GPIO.BOARD) else: GPIO.setmode(GPIO.BCM) for pin in self.pins: try: GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_UP) pass except RuntimeError: os.system("sudo chown tc /dev/gpiomem") GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_UP) def cbr(channel): self.longsleep = 0 GPIO.add_event_detect(pin, GPIO.BOTH, callback=cbr, bouncetime=20) global LIRC if LIRC: try: LIRC.init('pypilot') self.lirctime = False except: print 'failed to initialize lirc. is .lircrc missing?' LIRC = None def get(self, name): if self.client: self.client.get(name) def set_language(self, name): language = gettext.translation('pypilot_lcdclient', os.path.abspath(os.path.dirname(__file__)) + '/locale', languages=[name], fallback=True) global _ _ = language.ugettext self.config['language'] = name def save_config(self): try: file = open(self.configfilename, 'w') file.write(json.dumps(self.config) + '\n') except IOError: print 'failed to save config file:', self.configfilename def create_mainmenu(self): def value_edit(name, desc, signalk_name, value=False): min = self.value_list[signalk_name]['min'] max = self.value_list[signalk_name]['max'] step = (max-min)/100.0 def thunk(): self.range_edit=RangeEdit(name, desc, signalk_name, True, self, min, max, step) return self.range_edit.display if value: return name, thunk, lambda : (self.last_msg[signalk_name]-min) / (max - min), signalk_name return name, thunk def config_edit(name, desc, config_name, min, max, step): def thunk(): self.range_edit=RangeEdit(name, desc, config_name, False, self, min, max, step) return self.range_edit.display return name, thunk def gain(): self.menu = LCDMenu(self, _('Gain'), map(lambda gain : value_edit(gain[gain.find('.')+1:], gain, gain, True), gains), self.menu) return self.display_menu def level(): self.client.set('imu.alignmentCounter', 100) return self.display_page def calibrate(): def getheading(): self.get('imu.heading') try: return '%.1f' % self.last_msg['imu.heading'] except: return str(self.last_msg['imu.heading']) self.menu = LCDMenu(self, _('Calibrate'), [(_('level'), level), value_edit(_('heading'), getheading, 'imu.heading_offset'), (_('info'), lambda : self.display_calibrate_info)], self.menu) self.menu.display_hook = self.display_calibrate return self.display_menu def settings(): def mode(): def set_mode(name): def thunk(): self.client.set('ap.mode', name) self.menu = self.menu.adam() return self.display_control return thunk modes = [] index = 0 selection = 0 for mode in self.modes: if self.modes[mode](): modes.append((mode, set_mode(mode))) if mode == self.last_msg['ap.mode']: selection = index index+=1 self.menu = LCDMenu(self, _('Mode'), modes, self.menu) self.menu.selection = selection return self.display_menu def motor(): self.menu = LCDMenu(self, _('Motor'), [value_edit(_('min speed'), _('relative'), 'servo.min_speed'), value_edit(_('max speed'), _('relative'), 'servo.max_speed'), value_edit(_('max current'), _('amps'), 'servo.max_current'), value_edit(_('period'), _('seconds'), 'servo.period')], self.menu) return self.display_menu def filter(): self.menu = LCDMenu(self, _('Filter'), [value_edit(_('heading'), _('relative'), 'imu.heading_lowpass_constant'), value_edit(_("heading'"), _('relative'), 'imu.headingraterate_lowpass_constant'), value_edit(_("heading''"), _('relative'), 'imu.headingraterate_lowpass_constant')], self.menu) return self.display_menu def control(): self.menu = LCDMenu(self, _('Control'), [config_edit(_('small step'), _('degrees'), 'smallstep', 1, 5, 1), config_edit(_('big step'), _('degrees'), 'bigstep', 5, 20, 5)], self.menu) return self.display_menu def contrast(): self.range_edit = self.contrast_edit return self.range_edit.display def invert(): self.config['invert'] = not self.config['invert'] self.save_config() return self.display_menu def flip(): self.config['flip'] = not self.config['flip'] self.save_config() return self.display_menu def display(): self.menu = LCDMenu(self, _('Display'), [config_edit(_('contrast'), '', 'contrast', 30, 90, 1), (_('invert'), invert), (_('flip'), flip)], self.menu) return self.display_menu def language(): def set_language(name): def thunk(): self.set_language(name) self.save_config() self.create_mainmenu() return language() return thunk languages = [(_('English'), 'en'), (_('French'), 'fr'), (_('Spanish'), 'es')] index, selection = 0, 0 for lang in languages: if lang[1] == self.config['language']: selection = index index+=1 self.menu = LCDMenu(self, _('Language'), map(lambda lang : (lang[0], set_language(lang[1])), languages), self.menu) self.menu.selection = selection return self.display_menu self.menu = LCDMenu(self, _('Settings'), [(_('mode'), mode), (_('motor'), motor), # (_('filter'), filter), (_('control'), control), (_('display'), display), (_('language'), language)], self.menu) return self.display_menu self.menu = LCDMenu(self, _('Menu'), [(_('gain'), gain), (_('calibrate'), calibrate), (_('settings'), settings), (_('info'), lambda : self.display_info)]) self.info_page = 0 return self.display_menu def text(self, pos, text, size, crop=False): pos = int(pos[0]*self.surface.width), int(pos[1]*self.surface.height) size = int(size*self.surface.width/48) size = font.draw(self.surface, pos, text, size, self.bw, crop) return float(size[0])/self.surface.width, float(size[1])/self.surface.height def fittext(self, rect, text, wordwrap=False, fill='none'): #print 'fittext', text, wordwrap, fill if fill != 'none': self.surface.box(*(self.convrect(rect) + [fill])) metric_size = 16 if wordwrap: words = text.split(' ') spacewidth = font.draw(self.surface, False, ' ', metric_size, self.bw)[0] if len(words) < 2: # need at least 2 words to wrap return self.fittext(rect, text, False, fill) metrics = map(lambda word : (word, font.draw(self.surface, False, word, metric_size, self.bw)), words) widths = map(lambda metric : metric[1][0], metrics) maxwordwidth = apply(max, widths) totalwidth = sum(widths) + spacewidth * (len(words) - 1) size = 0 # not very efficient... just tries each x position # for wrapping to maximize final font size for wrappos in range(maxwordwidth, totalwidth+1): posx, posy = 0, 0 curtext = '' lineheight = 0 maxw = 0 for metric in metrics: word, (width, height) = metric if posx > 0: width += spacewidth if posx + width > wrappos: curtext += '\n' posx = 0 posy += lineheight lineheight = 0 if posx > 0: curtext += ' ' curtext += word lineheight = max(lineheight, height) posx += width maxw = max(maxw, posx) maxh = posy + lineheight s = maxw, maxh if s[0] == 0 or s[1] == 0: continue sw = self.surface.width * float(rect.width) / s[0] sh = self.surface.height * float(rect.height) / s[1] cursize = int(min(sw*metric_size, sh*metric_size)) if cursize > size: size = cursize text = curtext else: s = font.draw(self.surface, False, text, metric_size, self.bw) if s[0] == 0 or s[1] == 0: return 0, 0 sw = self.surface.width * float(rect.width) / s[0] sh = self.surface.height * float(rect.height) / s[1] size = int(min(sw*metric_size, sh*metric_size)) pos = int(rect.x*self.surface.width), int(rect.y*self.surface.height) size = font.draw(self.surface, pos, text, size, self.bw) return float(size[0])/self.surface.width, float(size[1])/self.surface.height def line(self, x1, y1, x2, y2): self.surface.line(x1, y1, x2, y2, black) def convbox(self, x1, y1, x2, y2): w, h = self.surface.width - 1, self.surface.height - 1 return [int(x1*w), int(y1*h), int(x2*w), int(y2*h)] def invertrectangle(self, rect): apply(self.surface.invert, self.convbox(rect.x, rect.y, rect.x+rect.width, rect.y+rect.height)) def convrect(self, rect): return self.convbox(rect.x, rect.y, rect.x+rect.width, rect.y+rect.height) def rectangle(self, rect, width = False): if not width: apply(self.surface.box, self.convrect(rect) + [white]) else: box = self.convrect(rect) apply(self.surface.invert, box) if width: w, h = self.surface.width - 1, self.surface.height - 1 px_width = int(max(1, min(w*width, h*width))) self.surface.invert(box[0]+px_width, box[1]+px_width, box[2]-px_width, box[3]-px_width) def connect(self): self.display_page = self.display_connecting watchlist = ['ap.enabled', 'ap.mode', 'ap.heading_command', 'gps.source', 'wind.source', 'servo.controller', 'servo.flags', 'imu.compass_calibration'] poll_list = ['ap.heading'] nalist = watchlist + poll_list + gains + \ ['imu.pitch', 'imu.heel', 'ap.runtime', 'ap.version', 'imu.heading', 'imu.compass_calibration_age', 'imu.heading_lowpass_constant', 'imu.headingrate_lowpass_constant', 'imu.headingraterate_lowpass_constant', 'servo.watts', 'servo.amp_hours'] + self.initial_gets self.last_msg = {} for name in nalist: self.last_msg[name] = _('N/A') for name in ['gps.source', 'wind.source']: self.last_msg[name] = 'none' self.last_msg['ap.heading_command'] = 0 self.last_msg['imu.heading_offset'] = 0 host = '' if len(sys.argv) > 1: host = sys.argv[1] def on_con(client): self.value_list = {} request = {'method' : 'list'} client.send(request) for name in watchlist: client.watch(name) try: self.client = SignalKClient(on_con, host) self.display_page = self.display_control print 'connected' for request in self.initial_gets: self.get(request) except: self.client = False time.sleep(1) def round_last_msg(self, name, places): n = 10**places try: return str(round(self.last_msg[name]*n)/n) except: return str(self.last_msg[name]) def have_compass(self): return True def have_gps(self): return self.last_msg['gps.source'] != 'none' def have_wind(self): return self.last_msg['wind.source'] != 'none' def have_true_wind(self): return self.have_gps() and self.have_wind() def display_wifi(self): wifi = False try: wlan0 = open('/sys/class/net/wlan0/operstate') line = wlan0.readline().rstrip() wlan0.close() if line == 'up': wifi = True except: pass if self.wifi != wifi: wifirect = rectangle(.3, .9, .6, .12) if wifi: self.fittext(wifirect, 'WIFI') else: self.surface.box(*(self.convrect(wifirect) + [black])) self.wifi = wifi def display_control(self): if not self.control: self.surface.fill(black) self.control = {'heading': ' ', 'heading_command': ' ', 'mode': False} def draw_big_number(pos, num, lastnum): def nr(x): try: s = str(int(round(x))) while len(s) < 3: s = ' ' + s return s except: return x num = nr(num) if lastnum: lastnum = nr(lastnum) if num == 'N/A' and lastnum != num: r = rectangle(pos[0], pos[1], 1, .4) self.fittext(r, num, False, black) return if self.surface.width < 256: size = 34 else: size = 30 for i in range(3): try: if num[i] == lastnum[i]: continue except: pass x = pos[0]+float(i)/3 self.surface.box(*(self.convrect(rectangle(x, pos[1], .34, .4)) + [black])) self.text((x, pos[1]), num[i], size, True) if type(self.last_msg['ap.heading']) == type(False): r = rectangle(0, 0, 1, .8) self.fittext(r, _('ERROR\ncompass or gyro failure!'), True, black) self.control['heading_command'] = 'no imu' else: draw_big_number((0,0), self.last_msg['ap.heading'], self.control['heading']) self.control['heading'] = self.last_msg['ap.heading'] mode = self.last_msg['ap.mode'] if 'OVERCURRENT' in self.last_msg['servo.flags']: self.overcurrent_time = time.time() if self.last_msg['servo.controller'] == 'none': if self.control['heading_command'] != 'no controller': self.fittext(rectangle(0, .4, 1, .35), _('WARNING no motor controller'), True, black) self.control['heading_command'] = 'no controller' elif time.time() - self.overcurrent_time < 5: # 5 seconds if self.control['heading_command'] != 'overcurrent': self.fittext(rectangle(0, .4, 1, .35), _('OVER CURRENT'), True, black) self.control['heading_command'] = 'overcurrent' elif 'OVERTEMP' in self.last_msg['servo.flags']: if self.control['heading_command'] != 'overtemp': self.fittext(rectangle(0, .4, 1, .35), _('OVER TEMP'), True, black) self.control['heading_command'] = 'overtemp' elif mode == 'gps' and not self.have_gps(): if self.control['heading_command'] != 'no gps': self.fittext(rectangle(0, .4, 1, .4), _('GPS not detected'), True, black) self.control['heading_command'] = 'no gps' elif (mode == 'wind' or mode == 'true wind') and not self.have_wind(): if self.control['heading_command'] != 'no wind': self.fittext(rectangle(0, .4, 1, .4), _('WIND not detected'), True, black) self.control['heading_command'] = 'no wind' else: # no warning, display the desired course or 'standby' if self.last_msg['ap.enabled'] != True: if self.control['heading_command'] != 'standby': r = rectangle(0, .4, 1, .34) self.fittext(r, _('standby'), False, black) self.control['heading_command'] = 'standby' else: if self.control['heading_command'] != self.last_msg['ap.heading_command']: draw_big_number((0,.4), self.last_msg['ap.heading_command'], self.control['heading_command']) self.control['heading_command'] = self.last_msg['ap.heading_command'] self.control['mode'] = False # refresh mode def modes(): return [self.have_compass(), self.have_gps(), self.have_wind(), self.have_true_wind()] warning = False if mode == 'compass': warning = False cal = self.last_msg['imu.compass_calibration'] if cal == 'N/A': ndeviation = 0 else: ndeviation = cal[1][0] def warncal(s): r = rectangle(0, .75, 1, .15) self.fittext(r, s, True, white) self.invertrectangle(r) self.control['mode'] = 'warning' if ndeviation == 0 and False: warncal(_('No Cal')) warning = True if ndeviation > 6: warncal(_('Bad Cal')) warning = True if not warning and \ (self.control['mode'] != mode or self.control['modes'] != modes()): self.control['mode'] = mode self.control['modes'] = modes() #print 'mode', self.last_msg['ap.mode'] modes = {'compass': ('C', self.have_compass, rectangle(0, .74, .25, .16)), 'gps': ('G', self.have_gps, rectangle(.25, .74, .25, .16)), 'wind': ('W', self.have_wind, rectangle(.5, .74, .25, .16)), 'true wind': ('T', self.have_true_wind, rectangle(.75, .74, .25, .16))} self.surface.box(*(self.convrect(rectangle(0, .74, 1, .18)) + [black])) for mode in modes: if modes[mode][1](): self.fittext(modes[mode][2], modes[mode][0]) if self.last_msg['ap.mode'] == mode: r = modes[mode][2] marg = .02 self.rectangle(rectangle(r.x-marg, r.y+marg, r.width-marg, r.height), .015) #self.control['mode'] = False # refresh mode self.display_wifi() def display_menu(self): self.surface.fill(black) self.menu.display() def display_calibrate(self): counter = self.last_msg['imu.alignmentCounter'] if counter: r = rectangle(0, 0, 1, .25) r.height = .2 self.fittext(r, ' %d%%' % (100-counter), False, black) r.width = 1-float(counter)/100 r.height = .25 self.invertrectangle(r) self.get('imu.alignmentCounter') self.fittext(rectangle(0, .86, .5, .14), self.round_last_msg('imu.pitch', 1)) self.fittext(rectangle(.5, .86, .5, .14), self.round_last_msg('imu.heel', 1)) self.get('imu.pitch') self.get('imu.heel') def display_connecting(self): self.surface.fill(black) self.fittext(rectangle(0, 0, 1, .4), _('connect to server'), True) dots = '' for i in range(self.connecting_dots): dots += '.' size = self.text((0, .4), dots, 12) self.connecting_dots += 1 if size[0] >= 1: self.connecting_dots = 0 self.display_wifi() def display_info(self): self.surface.fill(black) self.fittext(rectangle(0, 0, 1, .2), _('Info')) if self.info_page > 1: self.info_page = 0 y = .2 if self.info_page == 0: spacing = .11 v = self.round_last_msg('servo.watts', 3) runtime = self.last_msg['ap.runtime'][:7] ah = self.round_last_msg('servo.amp_hours', 3) items = [_('Watts'), v, _('Amp Hours'), ah, _('runtime'), runtime] self.get('servo.watts') self.get('servo.amp_hours') self.get('ap.runtime') else: spacing = .18 ver = self.last_msg['ap.version'] items = [_('version'), ver, _('author'), 'Sean D\'Epagnier'] self.get('ap.version') even, odd = 0, .05 for item in items: self.fittext(rectangle(0, y, 1, spacing+even), item, True) y += spacing + even even, odd = odd, even def display_calibrate_info(self): self.surface.fill(black) self.fittext(rectangle(0, 0, 1, .3), _('Calibrate Info'), True) deviation = _('N/A') try: cal = self.last_msg['imu.compass_calibration'] ndeviation = cal[0][1] #print ndeviation names = [(0, _('incomplete')), (.01, _('excellent')), (.02, _('good')), (.04, _('fair')), (.06, _('poor')), (1000, _('bad'))] for n in names: if ndeviation <= n[0]: deviation = n[1] break except: pass self.fittext(rectangle(0, .3, 1, .15), _('compass')) self.fittext(rectangle(0, .42, 1, .23), deviation) self.fittext(rectangle(0, .65, .4, .15), _('age')) self.fittext(rectangle(0, .8, 1, .2), self.last_msg['imu.compass_calibration_age'][:7]) #self.get('imu.compass_calibration') self.get('imu.compass_calibration_age') def display(self): self.display_page() if self.display_page != self.display_control: self.control = False self.wifi = False # status cursor w, h = self.surface.width, self.surface.height self.blink = self.blink[1], self.blink[0] size = h / 40 self.surface.box(w-size-1, h-size-1, w-1, h-1, self.blink[0]) def set(self, name, value): if self.client: self.client.set(name, value) def menu_back(self): if self.menu.prev: self.menu = self.menu.prev return self.display_menu return self.display_control def process_keys(self): if self.keypadup[AUTO]: # AUTO if self.last_msg['ap.enabled'] == False and self.display_page == self.display_control: self.set('ap.heading_command', self.last_msg['ap.heading']) self.set('ap.enabled', True) else: self.set('servo.command', 0) #stop self.set('ap.enabled', False) self.display_page = self.display_control if self.keypadup[SELECT]: if self.display_page == self.display_control: for t in range(len(self.modes_list)): next_mode = self.modes_list[0] self.modes_list = self.modes_list[1:] + [next_mode] if next_mode != self.last_msg['ap.mode'] and \ self.modes[next_mode](): self.client.set('ap.mode', next_mode) break else: self.menu = self.menu.adam() # reset to main menu self.display_page = self.display_control # for up and down keys providing acceration sign = -1 if self.keypad[DOWN] or self.keypadup[DOWN] or self.keypad[LEFT] or self.keypadup[LEFT] else 1 updownup = self.keypadup[UP] or self.keypadup[DOWN] updownheld = self.keypad[UP] > 10 or self.keypad[DOWN] > 10 speed = float(1 if updownup else min(10, .004*max(self.keypad[UP], self.keypad[DOWN])**2.5)) updown = updownheld or updownup if self.keypadup[LEFT] or self.keypadup[RIGHT]: updown = True speed = 10 if self.display_page == self.display_control: if self.keypadup[MENU] and self.surface: # MENU self.display_page = self.display_menu elif updown: # LEFT/RIGHT if self.last_msg['ap.enabled']: if self.keypadup[LEFT] or self.keypadup[RIGHT]: speed = self.config['bigstep'] else: speed = self.config['smallstep'] cmd = self.last_msg['ap.heading_command'] + sign*speed self.set('ap.heading_command', cmd) else: self.set('servo.command', sign*(speed+8.0)/20) elif self.display_page == self.display_menu: if self.keypadup[UP]: self.menu.selection -= 1 if self.menu.selection < 0: self.menu.selection = len(self.menu.items)-1 elif self.keypadup[DOWN]: self.menu.selection += 1 if self.menu.selection == len(self.menu.items): self.menu.selection = 0 elif self.keypadup[MENU]: self.display_page = self.menu.items[self.menu.selection][1]() elif self.display_page == self.display_info or \ self.display_page == self.display_calibrate_info: if self.keypadup[MENU]: self.display_page = self.display_menu if self.keypadup[UP] or self.keypadup[DOWN]: self.info_page += 1 elif self.range_edit and self.display_page == self.range_edit.display: if self.keypadup[MENU]: self.display_page = self.display_menu if not self.range_edit.signalk: self.save_config() elif updown: self.range_edit.move(sign*speed*.1) elif self.display_page == self.display_connecting: pass # no keys handled for this page else: print 'unknown display page', self.display_page for key in range(len(keynames)): if self.keypadup[key]: self.keypad[key] = self.keypadup[key] = False def key(self, k, down): if k >= 0 and k < len(self.pins): if down: self.keypad[k] = True else: self.keypadup[k] = True def glutkeydown(self, k, x, y): self.glutkey(k); def glutkeyup(self, k, x, y): self.glutkey(k, False) def glutkey(self, k, down=True): if k == 'q' or k == 27: exit(0) if k == ' ': key = keynames['auto'] elif k == '\n': key = keynames['menu'] elif k == '\t': key = keynames['select'] else: key = ord(k) - ord('1') self.key(key, down) def glutspecialdown(self, k, x, y): self.glutspecial(k); def glutspecialup(self, k, x, y): self.glutspecial(k, False) def glutspecial(self, k, down=True): if k == glut.GLUT_KEY_UP: self.key(keynames['up'], down) elif k == glut.GLUT_KEY_DOWN: self.key(keynames['down'], down) elif k == glut.GLUT_KEY_LEFT: self.key(keynames['left'], down) elif k == glut.GLUT_KEY_RIGHT: self.key(keynames['right'], down) def idle(self): self.get('ap.heading') if any(self.keypadup): self.longsleep = 0 if any(self.keypad): self.longsleep += 1 else: self.longsleep += 10 while self.longsleep > 20: dt = self.frameperiod / 10.0 time.sleep(dt) self.longsleep -= 1 # read from keys for pini in range(len(self.pins)): pin = self.pins[pini] value = True if False: f = open('/sys/class/gpio/gpio%d/value' % pin) a = f.readline() value = bool(int(a)) else: if GPIO: value = GPIO.input(pin) if not value and self.keypad[pini] > 0: self.keypad[pini] += 1 if pini in self.keystate and self.keystate[pini] != value: if value: self.keypadup[pini] = True else: self.keypad[pini] = 1 self.keystate[pini] = value if LIRC: if self.lirctime and time.time()- self.lirctime > .35: self.keypad[self.lirckey] = 0 self.keypadup[self.lirckey] = True self.lirctime = False #print 'keypad', self.keypad, self.keypadup while True: code = LIRC.nextcode(1) if not code: break repeat = code[0]['repeat'] lirc_mapping = {'up': UP, 'down': DOWN, 'left': LEFT, 'right': RIGHT, 'power': AUTO, 'select': SELECT, 'mute': MENU, 'tab': MENU} code = code[0]['config'] if not code in lirc_mapping: continue pini = lirc_mapping[code] if not self.surface and (pini == MENU or pini == SELECT): continue if repeat == 1: # ignore first repeat #if self.lirctime: # self.keypad[self.lirckey] = self.keypadup[self.lirckey] = False pass else: if repeat == 0: if self.lirctime: self.keypad[self.lirckey] = 0 self.keypadup[self.lirckey] = True self.keypad[pini] = 0 self.lirckey = pini; self.keypad[pini] += 1 self.lirctime = time.time() self.process_keys() while True: result = False if not self.client: self.connect() break try: result = self.client.receive_single() except Exception as e: print 'disconnected', e self.client = False if not result: break name, data = result #print name, ' = ', data, if 'value' in data: self.last_msg[name] = data['value'] for token in ['min', 'max']: if token in data: #print 'name', name, token, ' = ', data[token] if not name in self.value_list: self.value_list[name] = {} self.value_list[name][token] = data[token]
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase): ID_MESSAGES = 1000 ID_CALIBRATE_SERVO = 1001 ID_HEADING_OFFSET = 1002 ID_REQUEST_MSG = 1003 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.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.servoprocess = False self.fusionQPose = [1, 0, 0, 0] self.controltimes = {} def set_watches(self, client): if not client: return watchlist = [[ 'imu.fusionQPose', 'imu.alignmentCounter', 'imu.heading', 'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading_offset' ], [ 'imu.accel.calibration', 'imu.accel.calibration.age', 'imu.accel', 'imu.accel.calibration.sigmapoints', 'imu.accel.calibration.locked' ], [ 'imu.fusionQPose', 'imu.compass.calibration', 'imu.compass.calibration.age', 'imu.compass', 'imu.compass.calibration.sigmapoints', 'imu.compass.calibration.locked' ], [ 'servo.flags', 'servo.max_current', 'servo.max_controller_temp', 'servo.max_motor_temp', 'servo.current.factor', 'servo.current.offset', 'servo.voltage.factor', 'servo.voltage.offset', 'servo.max_slew_speed', 'servo.max_slew_slow', 'servo.gain' ], [ 'rudder.offset', 'rudder.scale', 'rudder.nonlinearity', 'rudder.range', 'rudder.calibration_state' ]] pageindex = 0 for pagelist in watchlist: for name in pagelist: client.watch(name, False) pageindex = self.m_notebook.GetSelection() for name in watchlist[pageindex]: client.watch(name) def on_con(self, client): self.set_watches(client) def receive_messages(self, event): if not self.client: try: self.client = SignalKClient(self.on_con, self.host, autoreconnect=False) except socket.error: self.timer.Start(5000) return try: msg = self.client.receive_single() while msg: self.receive_message(msg) msg = self.client.receive_single() self.timer.Start(50) except ConnectionLost: self.client = False def request_msg(self, event): if not self.client: return page_gets = [[], [], [], ['servo.voltage', 'servo.current'], ['rudder.angle']] i = self.m_notebook.GetSelection() for name in page_gets[i]: self.client.get(name) def UpdateControl(self, control, update): t = time.time() if not control in self.controltimes or t - self.controltimes[ control] > .5: update() self.controltimes[control] = t def UpdateLabel(self, label, value): self.UpdateControl(label, lambda: label.SetLabel(str(value))) def UpdatedSpin(self, dspin, value): self.UpdateControl(dspin, lambda: dspin.SetValue(value)) def receive_message(self, msg): name, data = msg value = data['value'] if 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() == 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() == 3: if name == 'servo.flags': self.UpdateLabel(self.stServoFlags, 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.UpdatedSpin(self.dsServoMaxCurrent, round3(value)) elif name == 'servo.max_controller_temp': self.UpdatedSpin(self.dsServoMaxControllerTemp, value) elif name == 'servo.max_motor_temp': self.UpdatedSpin(self.dsServoMaxMotorTemp, value) elif name == 'servo.current.factor': self.UpdatedSpin(self.dsServoCurrentFactor, round(value, 4)) elif name == 'servo.current.offset': self.UpdatedSpin(self.dsServoCurrentOffset, value) elif name == 'servo.voltage.factor': self.UpdatedSpin(self.dsServoVoltageFactor, round(value, 4)) elif name == 'servo.voltage.offset': self.UpdatedSpin(self.dsServoVoltageOffset, value) elif name == 'servo.max_slew_speed': self.UpdatedSpin(self.dsServoMaxSlewSpeed, value) elif name == 'servo.max_slew_slow': self.UpdatedSpin(self.dsServoMaxSlewSlow, value) elif name == 'servo.gain': self.UpdatedSpin(self.dsServoGain, value) elif name == 'servo.voltage': self.UpdateLabel(self.m_stServoVoltage, (str(round3(value)))) elif name == 'servo.current': self.UpdateLabel(self.m_stServoCurrent, (str(round3(value)))) elif self.m_notebook.GetSelection() == 4: 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) def servo_console(self, text): self.stServoCalibrationConsole.SetLabel( self.stServoCalibrationConsole.GetLabel() + text + '\n') def calibrate_servo_timer(self, event): if not self.servoprocess: return self.servoprocess.poll() print('servotimer', self.servoprocess.returncode, self.servoprocess.returncode == None) if self.servoprocess.returncode == None: self.servoprocess.communicate() line = self.servoprocess.stdout.readline() print('line', line) if line: self.servo_console(line) self.servo_timer.Start(150, True) else: self.bCalibrateServo.Enable() if self.servoprocess.returncode == 0: file = open('servo_calibration') calibration = json.loads(file.readline()) self.client.set('servo.calibration', calibration) self.servo_console('calibration sent.') else: self.servo_console('calibration failed.') self.servoprocess = False def PageChanged(self, event): self.set_watches(self.client) def onKeyPressAccel(self, event): self.onKeyPress(event, self.compass_calibration_plot) def onKeyPressCompass(self, event): self.onKeyPress(event, self.compass_calibration_plot) def onKeyPress(self, event, plot): signalk.scope_wx.wxglutkeypress(event, plot.special, plot.key) def onClearAccel(self, event): self.accel_calibration_plot.points = [] def onClearCompass(self, event): self.compass_calibration_plot.points = [] def onAccelCalibrationLocked(self, event): self.client.set('imu.accel.calibration.locked', self.cbAccelCalibrationLocked.GetValue()) def onCompassCalibrationLocked(self, event): self.client.set('imu.compass.calibration.locked', self.cbCompassCalibrationLocked.GetValue()) def onCalibrationLocked(self, sensor, ctrl): self.client.set('imu.' + sensor + '.calibration.locked', self.ctrl.GetValue()) def onMouseEventsAccel(self, event): self.AccelCalibration.SetFocus() self.onMouseEvents(event, self.AccelCalibration, self.accel_calibration_plot) def onMouseEventsCompass(self, event): self.CompassCalibration.SetFocus() self.onMouseEvents(event, self.CompassCalibration, self.compass_calibration_plot) def onMouseEvents(self, event, canvas, plot): pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \ pos[1] - self.lastmouse[1]) canvas.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: canvas.Refresh() while rotation > 0: plot.userscale /= .9 rotation -= 1 while rotation < 0: plot.userscale *= .9 rotation += 1 def onPaintGLAccel(self, event): self.onPaintGL(self.AccelCalibration, self.accel_calibration_plot, self.accel_calibration_glContext) def onPaintGLCompass(self, event): self.onPaintGL(self.CompassCalibration, self.compass_calibration_plot, self.compass_calibration_glContext) def onPaintGL(self, canvas, plot, context): wx.PaintDC(canvas) canvas.SetCurrent(context) plot.display() canvas.SwapBuffers() def onSizeGLAccel(self, event): self.accel_calibration_plot.reshape(event.GetSize().x, event.GetSize().y) def onSizeGLCompass(self, event): self.compass_calibration_plot.reshape(event.GetSize().x, event.GetSize().y) def StartAlignment(self): self.client.set('imu.alignmentCounter', 100) def onResetAlignment(self, event): self.client.set('imu.alignmentQ', [1, 0, 0, 0]) def onLevel(self, event): self.StartAlignment() def onIMUHeadingOffset(self, event): self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue()) self.heading_offset_timer.Stop() def onKeyPressBoatPlot(self, event): self.BoatPlot.SetFocus() k = '%c' % (event.GetKeyCode() & 255) if not event.GetModifiers() & wx.MOD_SHIFT: k = k.lower() self.BoatPlot.Refresh() def onMouseEventsBoatPlot(self, event): self.BoatPlot.SetFocus() pos = event.GetPosition() if event.LeftDown(): self.lastmouse = pos if event.Dragging(): dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1] q = pypilot.quaternion.angvec2quat( (dx**2 + dy**2)**.4 / 180 * math.pi, [dy, dx, 0]) self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q) self.BoatPlot.Refresh() self.lastmouse = pos rotation = event.GetWheelRotation() / 60 if rotation: self.BoatPlot.Refresh() while rotation > 0: self.boat_plot.Scale /= .9 rotation -= 1 while rotation < 0: self.boat_plot.Scale *= .9 rotation += 1 def onPaintGLBoatPlot(self, event): wx.PaintDC(self.BoatPlot) self.BoatPlot.SetCurrent(self.boat_plot_glContext) # stupid hack self.boat_plot.reshape(self.BoatPlot.GetSize().x, self.BoatPlot.GetSize().y) self.boat_plot.display(self.fusionQPose) self.BoatPlot.SwapBuffers() def onSizeGLBoatPlot(self, event): self.boat_plot.reshape(event.GetSize().x, event.GetSize().y) self.BoatPlot.Refresh() def onTextureCompass(self, event): self.boat_plot.texture_compass = event.IsChecked() self.BoatPlot.Refresh() def onIMUScope(self, event): host, port = self.client.host_port args = [ 'python', os.path.abspath(os.path.dirname(__file__)) + '/../signalk/scope_wx.py', host + ':' + str(port), 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading' ] subprocess.Popen(args) def onCalibrateServo(self, event): try: self.servoprocess = subprocess.Popen( ['python', 'servo_calibration.py', sys.argv[1]], stdout=subprocess.PIPE) self.servo_console('executed servo_calibration.py...') self.servo_timer.Start(150, True) self.bCalibrateServo.Disable() except OSError: self.servo_console('Failed to execute servo_calibration.py.\n') def onMaxCurrent(self, event): self.client.set('servo.max_current', event.GetValue()) def onMaxControllerTemp(self, event): self.client.set('servo.max_controller_temp', event.GetValue()) def onMaxMotorTemp(self, event): self.client.set('servo.max_motor_temp', event.GetValue()) def onCurrentFactor(self, event): self.client.set('servo.current.factor', event.GetValue()) def onCurrentOffset(self, event): self.client.set('servo.current.offset', event.GetValue()) def onVoltageFactor(self, event): self.client.set('servo.voltage.factor', event.GetValue()) def onVoltageOffset(self, event): self.client.set('servo.voltage.offset', event.GetValue()) def onMaxSlewSpeed(self, event): self.client.set('servo.max_slew_speed', event.GetValue()) def onMaxSlewSlow(self, event): self.client.set('servo.max_slew_slow', event.GetValue()) def onServoGain(self, event): self.client.set('servo.gain', event.GetValue()) def onServoAutoGain(self, event): if self.have_rudder: self.client.set('rudder.calibration_state', 'auto gain') else: wx.MessageDialog( self, _('Auto gain calibration requires a rudder sensor'), _('Warning'), wx.OK).ShowModal() def onRudderResetCalibration(self, event): self.client.set('rudder.calibration_state', 'reset') def onRudderCentered(self, event): self.client.set('rudder.calibration_state', 'centered') def onRudderStarboardRange(self, event): self.client.set('rudder.calibration_state', 'starboard range') def onRudderPortRange(self, event): self.client.set('rudder.calibration_state', 'port range') def onRudderRange(self, event): self.client.set('rudder.range', event.GetValue())
def nmea_bridge_process(pipe=False): import os sockets = [] watchlist = [ 'ap.enabled', 'ap.mode', 'ap.heading_command', 'imu/pitch', 'imu/roll', 'imu/heading_lowpass', 'gps.source', 'wind.speed', 'wind.direction', 'wind.source' ] def setup_watches(client, watch=True): for name in watchlist: client.watch(name, watch) def on_con(client): print 'nmea client connected' if sockets: setup_watches(client) # we actually use a local connection to the server to simplify logic print 'nmea try connections' while True: try: client = SignalKClient(on_con, 'localhost', autoreconnect=True) break except: time.sleep(2) print 'nmea connected' server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.setblocking(0) server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) port = 10110 try: server.bind(('0.0.0.0', port)) except: print 'nmea_bridge: bind failed.' exit(1) print 'listening on port', port, 'for nmea connections' server.listen(5) max_connections = 10 READ_ONLY = select.POLLIN | select.POLLHUP | select.POLLERR ap_enabled = 'N/A' ap_mode = 'N/A' ap_heading_command = 180 addresses = {} cnt = 0 poller = select.poll() poller.register(server, READ_ONLY) fd_to_socket = {server.fileno(): server} windspeed = 0 gps_source = wind_source = False while True: if sockets: timeout = 100 else: timeout = 10000 events = poller.poll(timeout) while events: event = events.pop() fd, flag = event sock = fd_to_socket[fd] if sock == server: connection, address = sock.accept() if len(sockets) == max_connections: connection.close() else: if not sockets: setup_watches(client) sock = LineBufferedNonBlockingSocket(connection) sockets.append(sock) print 'new connection: ', address addresses[sock] = address fd = sock.socket.fileno() fd_to_socket[fd] = sock poller.register(sock.socket, READ_ONLY) elif (flag & (select.POLLHUP | select.POLLERR)) or \ (flag & select.POLLIN and not sock.recv()): print 'lost connection: ', addresses[sock] sockets.remove(sock) # addresses.remove(sock) if not sockets: setup_watches(client, False) poller.unregister(sock.socket) fd = sock.socket.fileno() del fd_to_socket[fd] sock.socket.close() # elif flag & select.POLLOUT: # sock.flush() # if not sock.out_buffer: # poller.register(sock.socket, READ_ONLY) for sock in sockets: line = sock.readline() if not line: continue if line[:6] == '$GPRMC': if pipe and gps_source != 'internal': data = line[7:len(line) - 3].split(',') timestamp = float(data[0]) speed = float(data[6]) heading = float(data[7]) pipe.send( { 'gps': { 'timestamp': timestamp, 'track': heading, 'speed': speed } }, False) elif line[0] == '$' and line[3:6] == 'MVW': if pipe and wind_source != 'internal': winddata = wind.parse_nmea(line) if winddata: pipe.send({'wind': winddata}, False) elif line[0] == '$' and line[3:6] == 'APB': data = line[7:len(line) - 3].split(',') if not ap_enabled: client.set('ap.enabled', True) if ap_mode != 'gps': client.set('ap.mode', 'gps') if abs(ap_heading_command - float(data[7])) > .1: client.set('ap.heading_command', float(data[7])) msgs = client.receive() for name in msgs: data = msgs[name] value = data['value'] msg = False if name == 'ap.enabled': ap_enabled = value elif name == 'ap.mode': ap_mode = value elif name == 'ap.heading_command': ap_heading_command = value elif name == 'imu/pitch': msg = 'APXDR,A,%.3f,D,PTCH' % value elif name == 'imu/roll': msg = 'APXDR,A,%.3f,D,ROLL' % value elif name == 'imu/heading_lowpass': msg = 'APHDM,%.3f,M' % value elif name == 'gps.source': gps_source = value elif name == 'wind.speed': windspeed = value elif name == 'wind.direction': msg = 'APMWV,%.1f,R,%.1f,K,A' % (value, windspeed) elif name == 'wind.source': wind_source = value if msg: msg = '$' + msg + '*' + cksum(msg) + '\r\n' for sock in sockets: sock.send(msg) sock.flush()
class RayClient(): def __init__(self): self.client = False self.remote_key = 0 self.blinker_counter = 0 self.mode = MODE_STBY self.last_servo_command = 0 GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(SB, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Stand By: 1 GPIO.setup(AU, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Auto: 2 GPIO.setup(P1, GPIO.IN, pull_up_down=GPIO.PUD_UP) # +1: 4 GPIO.setup(P10, GPIO.IN, pull_up_down=GPIO.PUD_UP) # +10: 8 GPIO.setup(M10, GPIO.IN, pull_up_down=GPIO.PUD_UP) # -10: 16 GPIO.setup(M1, GPIO.IN, pull_up_down=GPIO.PUD_UP) # -1: 32 GPIO.setup(BUZZER, GPIO.OUT) GPIO.setup(BLINKER, GPIO.OUT) GPIO.output(BLINKER, 0) def connect(self): watchlist = [ 'ap.enabled', 'ap.mode', 'ap.pilot', 'ap.bell_server', 'ap.heading', 'ap.heading_command', 'gps.source', 'wind.source', 'servo.controller', 'servo.flags', 'ap.pilot.basic.P', 'ap.pilot.basic.I', 'ap.pilot.basic.D' ] self.last_msg = {} host = '' if len(sys.argv) > 1: host = sys.argv[1] def on_con(client): self.value_list = client.list_values(10) for name in watchlist: client.watch(name) try: self.client = SignalKClient(on_con, host) if self.value_list: print('connected') else: client.disconnect() raise 1 except Exception as e: print(e) self.client = False time.sleep(1) self.server = SignalKServer() def Register(_type, name, *args, **kwargs): return self.server.Register(_type(*([name] + list(args)), **kwargs)) ap_bell_server = Register( EnumProperty, 'ap.bell_server', '10.10.10.1', ['10.10.10.1', '10.10.10.2', '10.10.10.4', '192.168.178.129'], persistent=True) self.last_msg['ap.bell_server'] = ap_bell_server.value ap_pilot = Register(StringValue, 'ap.pilot', 'none') self.last_msg['ap.pilot'] = ap_pilot.value def last_val(self, name): if name in self.last_msg: return self.last_msg[name] return 'N/A' def set(self, name, value): if self.client: self.client.set(name, value) def get(self, name): if self.client: self.client.get(name) def bell(self, b): bell_server = self.last_val('ap.bell_server') print("bell_server=" + str(bell_server)) if (bell_server != 'N/A'): if (b == 1): file = '1bells.wav' if (b == 2): file = '2bells.wav' try: os.system('echo ' + file + ' | nc -w 1 ' + bell_server + ' 7000') except Exception as e: print('ex', e) self.last_bell = datetime.now() def beep(self, b): if (b == 1): GPIO.output(BUZZER, 1) time.sleep(0.05) GPIO.output(BUZZER, 0) if (b == 2): GPIO.output(BUZZER, 1) time.sleep(0.1) GPIO.output(BUZZER, 0) if (b == 3): self.beep(1) time.sleep(0.05) self.beep(1) if (b == 4): self.beep(3) time.sleep(0.1) self.beep(3) def adjust_gain(self, mode, factor): if (mode == MODE_P): gain = "P" if (mode == MODE_I): gain = "I" if (mode == MODE_D): gain = "D" gain_name = "ap.pilot." + self.last_val("ap.pilot") + "." + gain gain_name = gain_name.replace("pilot..", "") current_gain = self.last_val(gain_name) new_gain = current_gain * factor print gain_name + " = " + str(current_gain) + " * " + str( factor) + " = " + str(new_gain) SetSignalkValue(gain_name, new_gain) def adjust_heading(self, adjustment): name = "ap.heading_command" current_value = self.last_val(name) if current_value == "N/A": current_value = 0 new_value = current_value + adjustment print name + " = " + str(current_value) + " + " + str( adjustment) + " = " + str(new_value) SetSignalkValue(name, new_value) def doBlinker(self): if (self.blinker_counter != 1000): ap_enabled = self.last_val("ap.enabled") ap_mode = self.last_val("ap.mode") if (ap_enabled and ap_mode == 'compass' and self.mode not in [ MODE_P, MODE_I, MODE_D, MODE_GAINS, MODE_WAYPOINT_L, MODE_WAYPOINT_R ]): self.mode = MODE_AUTO if (ap_enabled and ap_mode == 'gps' and self.mode not in [ MODE_P, MODE_I, MODE_D, MODE_GAINS, MODE_WAYPOINT_L, MODE_WAYPOINT_R ]): self.mode = MODE_TRACK if (not ap_enabled): self.mode = MODE_STBY if (self.mode == MODE_STBY): light_on = (self.blinker_counter in [1, 2]) if (self.mode == MODE_AUTO): light_on = (self.blinker_counter not in [1, 2]) if (self.mode == MODE_TRACK): light_on = (self.blinker_counter not in [1, 2, 5, 6]) if (self.mode == MODE_GAINS): light_on = (self.blinker_counter % 6 > 3) if (self.mode in [MODE_P, MODE_I, MODE_D]): light_on = (self.blinker_counter not in [1, 2, 11, 12, 21, 22, 31, 32]) if (self.mode in [MODE_WAYPOINT_L, MODE_WAYPOINT_R]): light_on = (self.blinker_counter % 10 > 5) if ((datetime.now() - self.last_bell).total_seconds() > 5): if self.mode in [MODE_WAYPOINT_R]: self.bell(1) self.beep(3) else: self.bell(2) self.beep(4) if (light_on): GPIO.output(BLINKER, 1) else: GPIO.output(BLINKER, 0) self.blinker_counter = (self.blinker_counter + 1) % 40 def getMessages(self): # Listen out for SignalK messages; if they arrive, update them in self.last_msg dictionary while True: result = False if not self.client: self.connect() break try: result = self.client.receive_single() except Exception as e: print('disconnected', e) self.client = False if not result: break name, data = result if 'value' in data: self.last_msg[name] = data['value'] #print(str(name) + " = " + str(data['value'])) def handleKey(self, key): print("key = " + str(key) + ", mode = " + str(self.mode)) next_mode = self.mode # Standby key if (key == 1): if (self.mode in [ MODE_AUTO, MODE_P, MODE_I, MODE_D, MODE_TRACK, MODE_WAYPOINT_L, MODE_WAYPOINT_R ]): print "Stand by" self.set("ap.enabled", False) self.set("servo.command", 0) next_mode = MODE_STBY self.beep(2) if (self.mode == MODE_GAINS): next_mode = MODE_D print "Enter D:" # Auto key if (key == 2 and self.mode != MODE_AUTO): self.beep(1) print "Auto" print datetime.now() self.set("ap.heading_command", int(self.last_val("ap.heading"))) self.set("ap.enabled", True) self.set("ap.mode", "compass") print datetime.now() next_mode = MODE_AUTO # +1 if (key == 4): self.beep(1) if (self.mode == MODE_AUTO): print "+1" self.adjust_heading(+1) if (self.mode in [MODE_P, MODE_I, MODE_D]): self.adjust_gain(self.mode, FACTOR_LOW) if (self.mode in [MODE_STBY]): servo_command = -20 SetSignalkValue("servo.speed.max", SERVO_SPEED_SLOW) SetSignalkValue("servo.speed.min", SERVO_SPEED_SLOW) SetSignalkValue("servo.command", servo_command) self.last_servo_command = servo_command # -1 if (key == 32): self.beep(1) if (self.mode == MODE_AUTO): print "-1" self.adjust_heading(-1) if (self.mode == MODE_GAINS): next_mode = MODE_P print "Enter P:" if (self.mode in [MODE_P, MODE_I, MODE_D]): self.adjust_gain(self.mode, 1 / FACTOR_LOW) if (self.mode in [MODE_STBY]): servo_command = +20 SetSignalkValue("servo.speed.max", SERVO_SPEED_SLOW) SetSignalkValue("servo.speed.min", SERVO_SPEED_SLOW) SetSignalkValue("servo.command", servo_command) self.last_servo_command = servo_command # +10 if (key == 8): self.beep(2) if (self.mode == MODE_AUTO): print "+10" self.adjust_heading(+10) if (self.mode in [MODE_P, MODE_I, MODE_D]): self.adjust_gain(self.mode, FACTOR_MEDIUM) if (self.mode in [MODE_STBY]): servo_command = -1000 SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST) SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST) SetSignalkValue("servo.command", servo_command) self.last_servo_command = servo_command # -10 if (key == 16): self.beep(2) if (self.mode == MODE_AUTO): print "-10" self.adjust_heading(-10) if (self.mode == MODE_GAINS): next_mode = MODE_I print "Enter I:" if (self.mode in [MODE_P, MODE_I, MODE_D]): self.adjust_gain(self.mode, 1 / FACTOR_MEDIUM) if (self.mode in [MODE_STBY]): servo_command = +1000 SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST) SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST) SetSignalkValue("servo.command", servo_command) self.last_servo_command = servo_command # Track -10 & +10 if (key == 24 and self.mode in [MODE_AUTO, MODE_WAYPOINT_L, MODE_WAYPOINT_R]): self.beep(3) print "Track" SetSignalkValue("ap.enabled", True) SetSignalkValue("ap.mode", "gps") next_mode = MODE_TRACK # Tack Port -1 & -10 if (key == 48 and self.mode == MODE_AUTO): self.beep(3) print "Tack Port" adjust_heading(-100) # SetSignalkValue("ap.tack.direction", "port") # SetSignalkValue("ap.tack.state", "begin") # Tack Starboard +1 & +10 if (key == 12 and self.mode == MODE_AUTO): self.beep(3) print "Tack Starboard" adjust_heading(+100) # SetSignalkValue("ap.tack.direction", "starboard") # SetSignalkValue("ap.tack.state", "begin") # Set gains: +1 & -1 if (key == 36 and self.mode in [MODE_AUTO, MODE_TRACK, MODE_P, MODE_I, MODE_D]): self.beep(3) print "Choose gain" next_mode = MODE_GAINS # Artificial mode: Waypoint Arrival if (key == 1000 and self.mode in [MODE_TRACK]): print "Waypoint arrival, confirm with 'Track'" next_mode = MODE_WAYPOINT_R self.bell(1) if (key == 1001 and self.mode in [MODE_TRACK]): print "Waypoint arrival, confirm with 'Track'" next_mode = MODE_WAYPOINT_L self.bell(2) if self.mode != next_mode: blinker_counter = 1 self.mode = next_mode self.remote_key = 0 def processKeys(self): # wait for a button to be pressed. In the meantime, listen for SignalK messages and blink the LED: while (GPIO.input(SB) == 1 and GPIO.input(AU) == 1 and GPIO.input(P1) == 1 and GPIO.input(P10) == 1 and GPIO.input(M10) == 1 and GPIO.input(M1) == 1 and self.remote_key == 0): self.getMessages() self.doBlinker() time.sleep(0.05) try: with open('/tmp/remote', 'r') as myfile: line = myfile.read().replace("\n", "") print "remote=" + line os.remove('/tmp/remote') self.remote_key = int(line) except: self.remote_key = 0 # wait for a possible second button to be pressed in parallel, or at least for the button to be finished vibrating time.sleep(0.05) # store the key (or key combination) in one variable key = (1 - GPIO.input(SB)) + 2 * (1 - GPIO.input(AU)) + 4 * ( 1 - GPIO.input(P1)) + 8 * (1 - GPIO.input(P10)) + 16 * ( 1 - GPIO.input(M10)) + 32 * (1 - GPIO.input(M1)) + self.remote_key self.handleKey(key) # Wait for key to be lifted while (GPIO.input(SB) == 0 or GPIO.input(AU) == 0 or GPIO.input(P1) == 0 or GPIO.input(P10) == 0 or GPIO.input(M10) == 0 or GPIO.input(M1) == 0): self.doBlinker() time.sleep(0.05) if key in [4, 8, 16, 32] and self.mode in [MODE_STBY]: SetSignalkValue("servo.command", self.last_servo_command) # Key released SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST) SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST) # Immediately stop manual movement: if (self.mode in [MODE_STBY]): SetSignalkValue("servo.command", 0)