def work_pypilot(): #init compass mode = conf.get('PYPILOT', 'mode') if mode == 'disabled': print 'pypilot disabled ' return headingSK = conf.get('PYPILOT', 'translation_magnetic_h') attitudeSK = conf.get('PYPILOT', 'translation_attitude') SETTINGS_FILE = "RTIMULib" s = RTIMU.Settings(SETTINGS_FILE) imu = RTIMU.RTIMU(s) imuName = imu.IMUName() del imu del s if mode == 'imu': cmd = ['pypilot_boatimu', '-q'] elif mode == 'basic autopilot': # ensure no serial getty running os.system('sudo systemctl stop [email protected]') os.system('sudo systemctl stop [email protected]') cmd = ['pypilot'] try: translation_rate = float(conf.get('PYPILOT', 'translation_rate')) except: translation_rate = 1 conf.set('PYPILOT', 'translation_rate', '1') pid = os.fork() try: if pid == 0: os.execvp(cmd[0], cmd) print 'failed to launch', cmd exit(1) except: print 'exception launching pypilot' exit(1) print 'launched pypilot pid', pid time.sleep(3) # wait 3 seconds to launch client def on_con(client): print 'connected' if headingSK == '1': client.watch('imu.heading') if attitudeSK == '1': client.watch('imu.pitch') client.watch('imu.roll') client = False tick1 = time.time() while read_sensors: ret = os.waitpid(pid, os.WNOHANG) if ret[0] == pid: # should we respawn pypilot if it crashes? print 'pypilot exited' break # connect to pypilot if not connected try: if not client: client = SignalKClient(on_con, 'localhost') except: time.sleep(1) continue # not much to do without connection try: result = client.receive() except: print 'disconnected from pypilot' client = False continue Erg = Translate(result) SignalK='{"updates":[{"$source":"OPsensors.I2C.'+imuName+'","values":[' SignalK+=Erg[0:-1]+'}]}]}\n' sock.sendto(SignalK, ('127.0.0.1', 55557)) if mode == 'imu': if 'imu.heading' in result: value = result['imu.heading']['value'] hdm = str(pynmea2.HDM('AP', 'HDM', (str(value),'M')))+'\r\n' sock.sendto(hdm, ('127.0.0.1', 10110)) if 'imu.roll' in result: value = result['imu.roll']['value'] xdr_r = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','ROLL')))+'\r\n' sock.sendto(xdr_r, ('127.0.0.1', 10110)) if 'imu.pitch' in result: value = result['imu.pitch']['value'] xdr_p = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','PTCH')))+'\r\n' sock.sendto(xdr_p, ('127.0.0.1', 10110)) while True: dt = translation_rate - time.time() + tick1 if dt <= 0: break time.sleep(dt) tick1 = time.time() # cleanup print 'stopping pypilot pid:', pid try: os.kill(pid, 15) time.sleep(1) # wait one second to shut down pypilot except Exception, e: print 'exception stopping pypilot', e
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 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 NmeaBridgeProcess(multiprocessing.Process): def __init__(self): self.pipe, pipe = NonBlockingPipe('nmea pipe', True) self.sockets = False super(NmeaBridgeProcess, self).__init__(target=self.process, args=(pipe,)) def setup_watches(self, watch=True): watchlist = ['gps.source', 'wind.source', 'rudder.source', 'apb.source'] for name in watchlist: self.client.watch(name, watch) def receive_nmea(self, line, device, msgs): parsers = [] # optimization to only to parse sentences here that would be discarded # in the main process anyway because they are already handled by a source # with a higher priority than tcp tcp_priority = source_priority['tcp'] for name in nmea_parsers: if source_priority[self.last_values[name + '.source']] >= tcp_priority: parsers.append(nmea_parsers[name]) for parser in parsers: result = parser(line) if result: name, msg = result msg['device'] = line[1:3]+device msgs[name] = msg return def new_socket_connection(self, server): connection, address = server.accept() max_connections = 10 if len(self.sockets) == max_connections: connection.close() print('nmea server has too many connections') return if not self.sockets: self.setup_watches() self.pipe.send('sockets') sock = NMEASocket(connection) self.sockets.append(sock) #print('new nmea connection: ', address) self.addresses[sock] = address fd = sock.socket.fileno() self.fd_to_socket[fd] = sock self.poller.register(sock.socket, select.POLLIN) print('new nmea connection: ', address) def socket_lost(self, sock, fd): print('lost nmea connection: ', self.addresses[sock]) try: self.sockets.remove(sock) except: print('nmea sock not in sockets!') return self.pipe.send('lostsocket' + str(sock.socket.fileno())) if not self.sockets: self.setup_watches(False) self.pipe.send('nosockets') try: self.poller.unregister(fd) except Exception as e: print('nmea failed to unregister socket', e) try: del self.fd_to_socket[fd] except Exception as e: print('nmea failed to remove fd', e) sock.close() def client_message(self, name, value): self.last_values[name] = value 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() while True: time.sleep(2) try: self.client = SignalKClient(on_con, 'localhost', autoreconnect=True) break except: pass server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.setblocking(0) server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 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'} 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) print('lost') self.socket_lost(sock, fd) elif sock == server: self.new_socket_connection(server) elif sock == pipe: while True: # receive all messages in pipe msg = self.pipe.recv() if not msg: break msg += '\r\n' for sock in self.sockets: sock.send(msg) pass elif flag & select.POLLIN: if not sock.recv(): print('sock recv lost') self.socket_lost(sock, fd) else: while True: line = sock.readline() if not line: break self.receive_nmea(line, 'socket' + str(sock.socket.fileno()), msgs) else: print('nmea bridge unhandled poll flag', flag) t2 = time.time() if msgs: if self.pipe.send(msgs): ## try , False msgs = {} t3 = time.time() try: signalk_msgs = self.client.receive() for name in signalk_msgs: self.client_message(name, signalk_msgs[name]['value']) except Exception as e: print('nmea exception receiving:', e) t4 = time.time() for sock in self.sockets: sock.flush() t5 = time.time() if t5-t1 > .1: print('nmea process loop too slow:', t1-t0, t2-t1, t3-t2, t4-t3, t5-t4) else: dt = .1 - (t5 - t0) if dt > 0 and dt < .1: time.sleep(dt)
class NmeaBridgeProcess(multiprocessing.Process): def __init__(self): self.pipe, pipe = NonBlockingPipe('nmea pipe', True) self.sockets = False super(NmeaBridgeProcess, self).__init__(target=self.process, args=(pipe, )) def setup_watches(self, watch=True): watchlist = [ 'ap.enabled', 'ap.mode', 'ap.heading_command', 'gps.source', 'wind.source' ] for name in watchlist: self.client.watch(name, watch) def receive_nmea(self, line, msgs): parsers = [] if source_priority[ self.last_values['gps.source']] >= source_priority['tcp']: parsers.append(parse_nmea_gps) if source_priority[ self.last_values['wind.source']] >= source_priority['tcp']: parsers.append(parse_nmea_wind) for parser in parsers: result = parser(line) if result: name, msg = result msgs[name] = msg return def receive_apb(self, line, msgs): # also allow ap commands (should we allow via serial too??) ''' ** APB - Autopilot Sentence "B" ** 13 15 ** 1 2 3 4 5 6 7 8 9 10 11 12| 14| ** | | | | | | | | | | | | | | | ** $--APB,A,A,x.x,a,N,A,A,x.x,a,c--c,x.x,a,x.x,a*hh<CR><LF> ** ** 1) Status ** V = LORAN-C Blink or SNR warning ** V = general warning flag or other navigation systems when a reliable ** fix is not available ** 2) Status ** V = Loran-C Cycle Lock warning flag ** A = OK or not used ** 3) Cross Track Error Magnitude ** 4) Direction to steer, L or R ** 5) Cross Track Units, N = Nautical Miles ** 6) Status ** A = Arrival Circle Entered ** 7) Status ** A = Perpendicular passed at waypoint ** 8) Bearing origin to destination ** 9) M = Magnetic, T = True ** 10) Destination Waypoint ID ** 11) Bearing, present position to Destination ** 12) M = Magnetic, T = True ** 13) Heading to steer to destination waypoint ** 14) M = Magnetic, T = True ** 15) Checksum ''' # if line[3:6] == 'APB' and time.time() - self.last_apb_time > 1: self.last_apb_time = time.time() data = line[7:len(line) - 3].split(',') if self.last_values['ap.enabled']: mode = 'compass' if data[13] == 'M' else 'gps' if self.last_values['ap.mode'] != mode: self.client.set('ap.mode', mode) command = float(data[12]) xte = float(data[2]) xte = min(xte, 0.15) # maximum 0.15 miles if data[3] == 'L': xte = -xte command += 300 * xte # 30 degrees for 1/10th mile if abs(self.last_values['ap.heading_command'] - command) > .1: self.client.set('ap.heading_command', command) return True return False def new_socket_connection(self, server): connection, address = server.accept() max_connections = 10 if len(self.sockets) == max_connections: connection.close() print 'nmea server has too many connections' return if not self.sockets: self.setup_watches() self.pipe.send('sockets') sock = NMEASocket(connection) self.sockets.append(sock) #print 'new nmea connection: ', address self.addresses[sock] = address fd = sock.socket.fileno() self.fd_to_socket[fd] = sock self.poller.register(sock.socket, select.POLLIN) def socket_lost(self, sock): #print 'lost connection: ', self.addresses[sock] try: self.sockets.remove(sock) except: print 'sock not in sockets!' pass if not self.sockets: self.setup_watches(False) self.pipe.send('nosockets') try: self.poller.unregister(sock.socket) except Exception as e: print 'failed to unregister socket', e try: fd = sock.socket.fileno() del self.fd_to_socket[fd] except Exception as e: print 'failed to remove fd', e sock.close() def client_message(self, name, value): self.last_values[name] = value def process(self, pipe): import os #print 'nmea bridge on', os.getpid() self.pipe = pipe self.sockets = [] self.last_apb_time = time.time() def on_con(client): print 'nmea client connected' if self.sockets: self.setup_watches() while True: time.sleep(2) try: self.client = SignalKClient(on_con, 'localhost', autoreconnect=True) break except: pass server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.setblocking(0) server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 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 = { 'ap.enabled': False, 'ap.mode': 'N/A', 'ap.heading_command': 1000, 'gps.source': 'none', 'wind.source': 'none' } 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) elif sock == server: self.new_socket_connection(server) elif sock == pipe: while True: # receive all messages in pipe msg = self.pipe.recv() if not msg: break if not self.receive_apb(msg, msgs): msg += '\r\n' for sock in self.sockets: sock.send(msg) elif flag & select.POLLIN: if not sock.recv(): self.socket_lost(sock) else: while True: line = sock.readline() if not line: break if not self.receive_apb(line, msgs): self.receive_nmea(line, msgs) else: print 'nmea bridge unhandled poll flag', flag t2 = time.time() if msgs: if self.pipe.send(msgs): ## try , False msgs = {} t3 = time.time() try: signalk_msgs = self.client.receive() for name in signalk_msgs: self.client_message(name, signalk_msgs[name]['value']) except Exception, e: print 'nmea exception receiving:', e t4 = time.time() for sock in self.sockets: sock.flush() t5 = time.time() if t5 - t1 > .1: print 'nmea process loop too slow:', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4 else: dt = .1 - (t5 - t0) if dt > 0 and dt < .1: time.sleep(dt)
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()