def receive_messages(self, event): if not self.client: try: host, port = self.host_port self.client = pypilotClient(self.on_con, host, port, autoreconnect=False) self.timer.Start(50) except socket.error: self.timer.Start(1000) return refresh = False while True: result = False try: result = self.client.receive_single() except ConnectionLost: self.client = False return except: pass if not result: break if self.watches[result[0]] or result[0] == 'timestamp': if self.plot.read_data(result): refresh = True if refresh: self.glArea.Refresh()
def run(self): from signal import signal def cleanup(a, b): print('time spent load time', self.load_time.time()) print('time spent fit time', self.fit_time.time()) print('time spent total', self.total_time.time()) exit(0) signal(2, cleanup) # add predictions to the list of sensors for p in self.conf['predictions']: if not p in self.conf['sensors']: self.conf['sensors'].append(p) t0 = time.monotonic() print('connecting to', self.host) self.client = pypilotClient(self.host) watches = self.conf['sensors'] + self.conf['state'] + 'ap.enabled' + 'timestamp' for name in watches: client.watch(name) while True: self.receive() if time.monotonic() - t0 > 600: self.save()
def receive_messages(self, event): if not self.client: try: host, port = self.host_port self.client = pypilotClient(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("pypilot 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: t = str(type(self.controls[name])) if t == "<class 'wx._controls.Choice'>" or t == "<class 'wx._core.Choice'>": if not self.controls[name].SetStringSelection( value): print( 'warning, invalid choice value specified') elif t == "<class 'wx._controls.Slider'>" or t == "<class 'wx._core.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)
def main(): def on_con(client): print('conected to pypilot server') client.watch('imu.heading_lowpass') client.watch('imu.pitch') client.watch('imu.roll') try: subprocess.check_output(['systemctl', 'is-enabled', 'pypilot_boatimu']).decode(sys.stdin.encoding) pypilot_boatimu = True except: pypilot_boatimu = False try: subprocess.check_output(['systemctl', 'is-enabled', 'pypilot']).decode(sys.stdin.encoding) pypilot = True except: pypilot = False if pypilot_boatimu or pypilot: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) client = False while True: time.sleep(0.5) try: if not client: client = pypilotClient(on_con, 'localhost') except: time.sleep(3) continue try: result = client.receive() except: print('disconnected from pypilot server') client = False continue headingValue = '' rollValue = '' pitchValue = '' values = '' for i in result: if 'imu.heading_lowpass' in i: headingValue = result[i]['value']*0.017453293 if 'imu.roll' in i: rollValue = result[i]['value']*0.017453293 if 'imu.pitch' in i: pitchValue = result[i]['value']*0.017453293 if headingValue and rollValue and pitchValue: if pypilot_boatimu: values += '{"path": "navigation.headingMagnetic","value":'+str(headingValue)+'}' values += ',' values += '{"path": "navigation.attitude","value":{"roll":'+str(rollValue)+',"pitch":'+str(pitchValue)+',"yaw":null}}' SignalK = '{"updates":[{"$source":"OpenPlotter.I2C.pypilot","values":['+values+']}]}\n' sock.sendto(SignalK.encode('utf-8'), ('127.0.0.1', 20220))
def __init__(self): wx.Frame.__init__(self, None, title="pypilot client", size=(1000, 600)) host = '' if len(sys.argv) > 1: host = sys.argv[1] self.client = pypilotClient(host) self.connected = 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) self.Refresh(None) 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)
def __init__(self): super(CalibrationDialog, self).__init__(None) self.host = '' if len(sys.argv) > 1: self.host = sys.argv[1] self.client = False self.accel_calibration_plot = calibration_plot.AccelCalibrationPlot() self.accel_calibration_glContext = wx.glcanvas.GLContext( self.AccelCalibration) self.compass_calibration_plot = calibration_plot.CompassCalibrationPlot( ) self.compass_calibration_glContext = wx.glcanvas.GLContext( self.CompassCalibration) self.boat_plot = boatplot.BoatPlot() self.boat_plot_glContext = wx.glcanvas.GLContext(self.BoatPlot) self.lastmouse = False self.alignment_count = 0 self.timer = wx.Timer(self, self.ID_MESSAGES) self.timer.Start(50) self.Bind(wx.EVT_TIMER, self.receive_messages, id=self.ID_MESSAGES) self.heading_offset_timer = wx.Timer(self, self.ID_HEADING_OFFSET) self.Bind(wx.EVT_TIMER, lambda e: self.sHeadingOffset.SetValue( round3(self.pypilot_heading_offset)), id=self.ID_HEADING_OFFSET) self.have_rudder = False self.fusionQPose = [1, 0, 0, 0] self.alignmentQ = [1, 0, 0, 0] self.controltimes = {} self.client = pypilotClient(self.host) # clear out plots self.accel_calibration_plot.points = [] self.compass_calibration_plot.points = [] self.settings = {} self.set_watches()
def receive_messages(self, event): if not self.client: try: self.client = pypilotClient(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 except Exception as e: print(e)
def receive(self): if self.playback_file: line = self.playback_file.readline() if not line: print('end of file') exit(0) msg = json.loads(line) for name in msg: self.receive_single(name, msg[name]) return if not self.client: print('connecting to', self.host) # couldn't load try to connect watches = self.conf['sensors'] + list(self.conf['state']) watches.append('ap.enabled') watches.append('timestamp') def on_con(client): for name in watches: client.watch(name) self.client = pypilotClient(on_con, self.host, autoreconnect=False) msg = self.client.receive_single(1) while msg: if self.record_file: d = {msg[0]: msg[1]['value']} self.record_file.write(json.dumps(d) + '\n') self.record_file.lines += 1 if self.record_file.lines % 100 == 0: sys.stdout.write('recording ' + str(self.record_file.lines) + '\r') sys.stdout.flush() else: name, data = msg value = data['value'] self.receive_single(name, value) msg = self.client.receive_single(-1)
def connect(self): for name in ['gps.source', 'wind.source']: self.last_msg[name] = 'none' self.last_msg['ap.enabled'] = False self.last_msg['ap.heading_command'] = 0 self.last_msg['imu.heading_offset'] = 0 if self.config['remote']: host = self.config['host'] else: host = 'localhost' def on_con(client): self.value_list = client.list_values(10) self.watchlist = ['ap.enabled', 'ap.heading_command' ] + self.lcd.watchlist for name in self.watchlist: client.watch(name) for request in self.lcd.initial_gets: client.get(request) try: self.client = pypilotClient(on_con, host) if self.value_list: print('connected') self.web.set_status('connected') else: self.client.disconnect() print('no value list!') self.client = False self.web.set_status('disconnected') except Exception as e: print('hat exception', e) self.client = False time.sleep(1)
def __init__(self): # read config self.config = { 'remote': False, 'host': '127.0.0.1', 'actions': {}, 'pi.ir': True, 'arduino.ir': False, 'arduino.nmea.in': False, 'arduino.nmea.out': False, 'arduino.nmea.baud': 4800, 'lcd': {} } self.configfilename = os.getenv('HOME') + '/.pypilot/hat.conf' print('loading config file:', self.configfilename) try: file = open(self.configfilename) config = pyjson.loads(file.read()) file.close() for name in config: self.config[name] = config[name] except Exception as e: print('config failed:', e) try: configfile = '/proc/device-tree/hat/custom_0' f = open(configfile) hat_config = pyjson.loads(f.read()) f.close() print('loaded device tree hat config') if not 'hat' in self.config or hat_config != self.config['hat']: self.config['hat'] = hat_config print('writing device tree hat to hat.conf') self.write_config() except Exception as e: print('failed to load', configfile, ':', e) if not 'hat' in self.config: if os.path.exists('/dev/spidev0.0'): print( 'assuming original 26 pin tinypilot with nokia5110 display' ) self.config['hat'] = { 'lcd': { 'driver': 'nokia5110', 'port': '/dev/spidev0.0' }, 'lirc': 'gpio4' } self.write_config() self.servo_timeout = time.monotonic() + 1 self.last_msg = {} self.last_msg['ap.enabled'] = False self.last_msg['ap.heading_command'] = 0 if self.config['remote']: host = self.config['host'] else: host = 'localhost' self.client = pypilotClient(host) self.client.registered = False self.watchlist = ['ap.enabled', 'ap.heading_command'] for name in self.watchlist: self.client.watch(name) self.lcd = lcd.LCD(self) self.gpio = gpio.gpio() self.arduino = Arduino(self) self.poller = select.poll() self.poller.register(self.arduino.pipe, select.POLLIN) self.lirc = lircd.lirc(self.config) self.lirc.registered = False self.keytimes = {} self.keytimeouts = {} # keypad for lcd interface self.actions = [] keypadnames = [ 'auto', 'menu', 'port1', 'starboard1', 'select', 'port10', 'starboard10', 'tack', 'dodge_port', 'dodge_starboard' ] for i in range(len(keypadnames)): self.actions.append(ActionKeypad(self.lcd, i, keypadnames[i])) # stateless actions for autopilot control self.actions += [ ActionEngage(self), ActionPypilot(self, 'disengage', 'ap.enabled', False), ActionHeading(self, 1), ActionHeading(self, -1), ActionHeading(self, 2), ActionHeading(self, -2), ActionHeading(self, 5), ActionHeading(self, -5), ActionHeading(self, 10), ActionHeading(self, -10), ActionPypilot(self, 'compassmode', 'ap.mode', 'compass'), ActionPypilot(self, 'gpsmode', 'ap.mode', 'gps'), ActionPypilot(self, 'windmode', 'ap.mode', 'wind'), ActionPypilot(self, 'center', 'servo.position', 0), ActionTack(self, 'tackport', 'port'), ActionTack(self, 'tackstarboard', 'starboard'), ActionNone() ] for action in self.actions: if action.name in self.config['actions']: action.keys = self.config['actions'][action.name] self.web = Web(self) def cleanup(signal_number, frame=None): print('got signal', signal_number, 'cleaning up', os.getpid()) childpids = [] processes = [self.arduino, self.web] for process in processes: if process.process: childpids.append(process.process.pid) if signal_number == signal.SIGCHLD: pid = os.waitpid(-1, os.WNOHANG) if not pid[0] in childpids: print('subprocess returned', pid, childpids) # flask or system makes process at startup that dies return print('child process', pid, childpids) while childpids: pid = childpids.pop() #print('kill!', pid, childpids, os.getpid()) try: os.kill(pid, signal.SIGTERM) # get backtrace except ProcessLookupError: pass # ok, process is already terminated sys.stdout.flush() for process in processes: process.process = False if signal_number != 'atexit': raise KeyboardInterrupt # to get backtrace on all processes sys.stdout.flush() for s in range(1, 16): if s != 9 and s != 13: signal.signal(s, cleanup) signal.signal(signal.SIGCHLD, cleanup) import atexit atexit.register(lambda: cleanup('atexit'))
def CalibrationProcess(cal_pipe): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print( 'warning, failed to make calibration process idle, trying renice') if os.system("renice 20 %d" % os.getpid()): print('warning, failed to renice calibration process') accel_cal = SigmaPoints(.05**2, 12, 10) compass_cal = SigmaPoints(1.1**2, 24, 3) accel_calibration = [0, 0, 0, 1] norm = [0, 0, 1] def on_con(client): client.watch('imu.alignmentQ') client.watch('imu.compass.calibration') while True: time.sleep(2) try: client = pypilotClient(on_con, 'localhost', autoreconnect=True) break except Exception as e: print('nmea process failed to connect pypilot', e) def debug(name): def debug_by_name(*args): s = '' for a in args: s += str(a) + ' ' if client: client.set('imu.' + name + '.calibration.log', s) return debug_by_name while True: t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: # receive pypilot messages msg = client.receive() for name in msg: value = msg[name]['value'] if name == 'imu.alignmentQ' and value: norm = quaternion.rotvecquat([0, 0, 1], value) compass_cal.Reset() elif name == 'imu.compass.calibration': compass_calibration = value[0] # receive calibration data p = cal_pipe.recv(1) if p: if 'accel' in p: accel_cal.AddPoint(p['accel']) addedpoint = True if 'compass' in p: compass_cal.AddPoint(p['compass'], p['down']) addedpoint = True # send updated sigmapoints as well cals = {'accel': accel_cal, 'compass': compass_cal} for name in cals: cal = cals[name] if cal.Updated(): points = cal.Points() client.set('imu.' + name + '.calibration.sigmapoints', points) if not addedpoint: # don't bother to run fit if no new data continue accel_cal.RemoveOlder(10 * 60) # 10 minutes fit = FitAccel(debug('accel'), accel_cal) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration[:3]) if dist > .01: # only update when bias changes more than this if dist > .08: # reset compass cal from large change in accel bias compass_cal.Reset() accel_calibration = fit[0] client.set('imu.accel.calibration', fit) compass_cal.RemoveOlder(20 * 60) # 20 minutes fit = FitCompass(debug('compass'), compass_cal, compass_calibration, norm) if fit: client.set('imu.compass.calibration', fit) compass_calibration = fit[0]
def process(self, pipe): import os self.pipe = pipe self.sockets = [] def on_con(client): print('nmea ready for connections') if self.sockets: self.setup_watches() client.watch('nmea.client') while True: time.sleep(2) try: self.client = pypilotClient(on_con, 'localhost', autoreconnect=True) break except Exception as e: print('nmea process failed to connect pypilot', e) server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.setblocking(0) server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.client_socket = False port = DEFAULT_PORT 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) self.last_values = { 'gps.source': 'none', 'wind.source': 'none', 'rudder.source': 'none', 'apb.source': 'none', 'nmea.client': '' } self.addresses = {} cnt = 0 self.poller = select.poll() self.poller.register(server, select.POLLIN) self.poller.register(pipe, select.POLLIN) self.fd_to_socket = {server.fileno(): server, pipe.fileno(): pipe} msgs = {} while True: timeout = 100 if self.sockets else 10000 t0 = time.time() events = self.poller.poll(timeout) t1 = time.time() while events: fd, flag = events.pop() sock = self.fd_to_socket[fd] if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL): if sock == server: print('nmea bridge lost server connection') exit(2) if sock == pipe: print('nmea bridge pipe to autopilot') exit(2) self.socket_lost(sock, fd) elif sock == server: self.new_socket_connection(*server.accept()) elif sock == pipe: while True: # receive all messages in pipe msg = self.pipe.recv() if not msg: break msg += '\r\n' # relay nmea message from server to all tcp sockets for sock in self.sockets: sock.send(msg) pass elif flag & select.POLLIN: if not sock.recv(): self.socket_lost(sock, fd) else: while True: line = sock.readline() if not line: break self.receive_nmea(line, 'socket' + str(sock.uid), msgs) else: print('nmea bridge unhandled poll flag', flag) t2 = time.time() # send any parsed nmea messages the server might care about if msgs: if self.pipe.send(msgs): msgs = {} t3 = time.time() # receive pypilot messages try: pypilot_msgs = self.client.receive() for name in pypilot_msgs: value = pypilot_msgs[name]['value'] self.last_values[name] = value except Exception as e: print('nmea exception receiving:', e) t4 = time.time() # flush sockets for sock in self.sockets: sock.flush() t5 = time.time() # reconnect client tcp socket nmea_client = self.last_values['nmea.client'] if self.client_socket: if self.client_socket.nmea_client != nmea_client: self.client_socket.socket.close( ) # address has changed, close connection elif nmea_client: try: self.connect_client(nmea_client) except Exception as e: print('failed to create nmea socket as host:port', nmea_client, e) self.last_values[ 'nmea.client'] = '' # don't try until changed t6 = time.time() # run tcp nmea traffic at rate of 10hz if t6 - t1 > .1: print('nmea process loop too slow:', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4, t6 - t5) else: dt = .1 - (t6 - t0) if dt > 0 and dt < .1: time.sleep(dt)
def on_connect(self): print('Client connected', request.sid) client = pypilotClient() self.clients[request.sid] = client
print('ERROR!', down, cal_sphere, e) glEnd() self.draw_points() if __name__ == '__main__': host = False if len(sys.argv) > 1: host = sys.argv[1] watchlist = [ 'imu.accel', 'imu.compass', 'imu.compass.calibration', 'imu.compass.calibration', 'imu.compass.calibration.sigmapoints', 'imu.fusionQPose' ] client = pypilotClient(host) for name in watchlist: client.watch(name) plot = CompassCalibrationPlot() def display(): plot.display() glutSwapBuffers() last = False def mouse(button, state, x, y): if button == GLUT_LEFT_BUTTON and state == GLUT_DOWN: global last last = x, y
if __name__ == '__main__': host = '' if len(sys.argv) > 1: host = sys.argv[1] def on_con(client): watchlist = [ 'imu.accel', 'imu.compass', 'imu.compass.calibration', 'imu.compass.calibration', 'imu.compass.calibration.sigmapoints', 'imu.fusionQPose', 'imu.alignmentQ' ] for name in watchlist: client.watch(name) client = pypilotClient(on_con, host, autoreconnect=True) plot = CompassCalibrationPlot() def display(): plot.display() glutSwapBuffers() last = False def mouse(button, state, x, y): if button == GLUT_LEFT_BUTTON and state == GLUT_DOWN: global last last = x, y def motion(x, y): global last