class AutomaticCalibrationProcess(): def __init__(self, server): if True: # direct connection to send raw sensors to calibration process is more # efficient than routing through server (save up to 2% cpu on rpi zero) self.cal_pipe, self.cal_pipe_process = NonBlockingPipe('cal pipe', True, sendfailok=True) else: self.cal_pipe, self.cal_pipe_process = False, False # use client self.client = pypilotClient(server) self.process = multiprocessing.Process(target=CalibrationProcess, args=(self.cal_pipe_process, self.client), daemon=True) self.process.start() self.cal_ready = False def calibration_ready(self): if self.cal_ready: return True if self.cal_pipe.recv(): self.cal_ready = True return True return False def __del__(self): #print(_('terminate calibration process')) self.process.terminate()
class nmeaBridge(object): def __init__(self, server): self.client = pypilotClient(server) self.multiprocessing = server.multiprocessing self.pipe, self.pipe_out = NonBlockingPipe('nmea pipe', self.multiprocessing) if self.multiprocessing: self.process = multiprocessing.Process(target=self.nmea_process, daemon=True) self.process.start() else: self.process = False self.setup() def setup(self): self.sockets = [] self.nmea_client = self.client.register( Property('nmea.client', '', persistent=True)) self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server.setblocking(0) self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.client_socket = False port = DEFAULT_PORT while True: try: self.server.bind(('0.0.0.0', port)) break except: print('nmea server on port %d: bind failed.', port) time.sleep(1) print('listening on port', port, 'for nmea connections') self.server.listen(5) self.failed_nmea_client_time = 0 self.last_values = { 'gps.source': 'none', 'wind.source': 'none', 'rudder.source': 'none', 'apb.source': 'none' } for name in self.last_values: self.client.watch(name) self.addresses = {} cnt = 0 self.poller = select.poll() self.poller.register(self.server, select.POLLIN) self.fd_to_socket = {self.server.fileno(): self.server} self.poller.register(self.client.connection, select.POLLIN) self.fd_to_socket[self.client.connection.fileno()] = self.client if self.multiprocessing: self.poller.register(self.pipe, select.POLLIN) self.fd_to_socket[self.pipe.fileno()] = self.pipe self.msgs = {} 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): 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'] = device + line[1:3] self.msgs[name] = msg return def new_socket_connection(self, connection, address): 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, address) self.sockets.append(sock) self.addresses[sock] = address fd = sock.socket.fileno() self.fd_to_socket[fd] = sock self.poller.register(sock.socket, select.POLLIN) return sock def socket_lost(self, sock, fd): if sock == self.client_socket: self.client_socket = False try: self.sockets.remove(sock) except: print('nmea sock not in sockets!') return self.pipe.send('lostsocket' + str(sock.uid)) 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) try: del self.addresses[sock] except Exception as e: print('nmea failed to remove address', e) sock.close() def connect_client(self): if not ':' in self.nmea_client.value: return host, port = self.nmea_client.value.split(':') port = int(port) try: s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) tc0 = time.monotonic() s.connect((host, port)) print('connected to', host, port, 'in', time.monotonic() - tc0, 'seconds') self.client_socket = self.new_socket_connection( s, self.nmea_client.value) self.client_socket.nmea_client = self.nmea_client.value except Exception as e: print('nmea client failed to connect to', self.nmea_client.value, ':', e) self.client_socket = False def nmea_process(self): print('nmea process', os.getpid()) self.setup() while True: t0 = time.monotonic() timeout = 100 if self.sockets else 10000 self.poll(timeout) def receive_pipe(self): while True: # receive all messages in pipe msg = self.pipe.recv() if not msg: return # relay nmea message from server to all tcp sockets for sock in self.sockets: sock.write(msg + '\r\n') def poll(self, timeout=0): t0 = time.monotonic() events = self.poller.poll(timeout) t1 = time.monotonic() if t1 - t0 > timeout: print('poll took too long in nmea process!') while events: fd, flag = events.pop() sock = self.fd_to_socket[fd] if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL): if sock == self.server: print('nmea bridge lost server connection') exit(2) if sock == self.pipe: print('nmea bridge pipe to autopilot') exit(2) self.socket_lost(sock, fd) elif sock == self.server: self.new_socket_connection(*self.server.accept()) elif sock == self.pipe: self.receive_pipe() elif sock == self.client: pass # wake from poll elif flag & select.POLLIN: if not sock.recvdata(): self.socket_lost(sock, fd) else: while True: line = sock.readline() if not line: break self.receive_nmea(line, 'socket' + str(sock.uid)) else: print('nmea bridge unhandled poll flag', flag) # if we are not multiprocessing, the pipe won't be pollable, so receive any data now if not self.process: self.receive_pipe() t2 = time.monotonic() # send any parsed nmea messages the server might care about if self.msgs: #print('nmea msgs', self.msgs) if self.pipe.send(self.msgs): self.msgs = {} t3 = time.monotonic() # receive pypilot messages pypilot_msgs = self.client.receive() for name in pypilot_msgs: value = pypilot_msgs[name] self.last_values[name] = value #except Exception as e: # print('nmea exception receiving:', e) t4 = time.monotonic() # flush sockets for sock in self.sockets: sock.flush() t5 = time.monotonic() # reconnect client tcp socket if self.client_socket: if self.client_socket.nmea_client != self.nmea_client.value: self.client_socket.socket.close( ) # address has changed, close connection elif t5 - self.failed_nmea_client_time > 20: try: self.connect_client() except Exception as e: print('failed to create nmea socket as host:port', self.nmea_client.value, e) self.failed_nmea_client_time = t5 t6 = time.monotonic() if t6 - t1 > .1: print('nmea process loop too slow:', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4, t6 - t5)
class nmeaBridge(object): def __init__(self, server): self.client = pypilotClient(server) self.multiprocessing = server.multiprocessing self.pipe, self.pipe_out = NonBlockingPipe('nmea pipe', self.multiprocessing) if self.multiprocessing: self.process = multiprocessing.Process(target=self.nmea_process, daemon=True) self.process.start() else: self.process = False self.setup() def setup(self): self.sockets = [] self.nmea_client = self.client.register( Property('nmea.client', '', persistent=True)) self.client_socket_warning_address = False self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server.setblocking(0) self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.connecting_client_socket = False self.client_socket = False port = DEFAULT_PORT while True: try: self.server.bind(('0.0.0.0', port)) break except: print( _('nmea server on port') + (' %d: ' % port) + _('bind failed.')) time.sleep(1) print(_('listening on port'), port, _('for nmea connections')) self.server.listen(5) self.client_socket_nmea_address = False self.nmea_client_connect_time = 0 self.last_values = { 'gps.source': 'none', 'wind.source': 'none', 'rudder.source': 'none', 'apb.source': 'none', 'water.source': 'none' } for name in self.last_values: self.client.watch(name) self.addresses = {} cnt = 0 self.poller = select.poll() self.poller.register(self.server, select.POLLIN) self.fd_to_socket = {self.server.fileno(): self.server} self.poller.register(self.client.connection, select.POLLIN) self.fd_to_socket[self.client.connection.fileno()] = self.client if self.multiprocessing: self.poller.register(self.pipe, select.POLLIN) self.fd_to_socket[self.pipe.fileno()] = self.pipe self.msgs = {} 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, sock): device = 'socket' + str(sock.uid) parsers = [] # if we receive a "special" pypilot nmea message from this # socket, then mark it to rebroadcast to other nmea sockets # normally only nmea data received from serial ports is broadcast if not sock.broadcast: if line == '$PYPBS*48': sock.broadcast = True return else: for s in self.sockets: if s != sock: s.write(line + '\r\n') # 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 self.msgs[name] = msg return def new_socket_connection(self, connection, address): 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, address) # normally don't re-transmit nmea data received from sockets # if it is marked to broadcast, then data received will re-transmit sock.broadcast = False self.sockets.append(sock) self.addresses[sock] = address fd = sock.socket.fileno() self.fd_to_socket[fd] = sock self.poller.register(sock.socket, select.POLLIN) return sock def socket_lost(self, sock, fd): #print('nmea socket lost', fd, sock, self.connecting_client_socket) if sock == self.connecting_client_socket: self.close_connecting_client() return if sock == self.client_socket: print(_('nmea client lost connection')) self.client_socket = False try: self.sockets.remove(sock) except: print(_('nmea sock not in sockets!')) return self.pipe.send('lostsocket' + str(sock.uid)) 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) try: del self.addresses[sock] except Exception as e: print(_('nmea failed to remove address'), e) sock.close() def connect_client(self): if self.client_socket: # already connected if self.client_socket_nmea_address != self.nmea_client.value: self.client_socket.socket.close( ) # address has changed, close connection return timeout = 30 t = time.monotonic() if self.client_socket_nmea_address != self.nmea_client.value: self.nmea_client_connect_time = t - timeout + 5 # timeout sooner if it changed self.client_socket_nmea_address = self.nmea_client.value if t - self.nmea_client_connect_time < timeout: return self.nmea_client_connect_time = t if not self.nmea_client.value: return if not ':' in self.nmea_client.value: self.warn_connecting_client(_('invalid value')) return hostport = self.nmea_client.value.split(':') host = hostport[0] port = hostport[1] self.client_socket = False def warning(e, s): self.warn_connecting_client(_('connect error') + ' : ' + str(e)) s.close() try: port = int(port) if self.connecting_client_socket: self.close_connecting_client() s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.setblocking(0) s.connect((host, port)) self.warn_connecting_client('connected without blocking') self.client_connected(s) except OSError as e: import errno if e.args[0] is errno.EINPROGRESS: self.poller.register(s, select.POLLOUT) self.fd_to_socket[s.fileno()] = s self.connecting_client_socket = s return warning(e, s) except Exception as e: warning(e, s) def warn_connecting_client(self, msg): if self.client_socket_warning_address != self.client_socket_nmea_address: print('nmea client ' + msg, self.client_socket_nmea_address) self.client_socket_warning_address = self.client_socket_nmea_address def close_connecting_client(self): self.warn_connecting_client(_('failed to connect')) fd = self.connecting_client_socket.fileno() self.poller.unregister(fd) del self.fd_to_socket[fd] self.connecting_client_socket.close() self.connecting_client_socket = False def client_connected(self, connection): print(_('nmea client connected'), self.client_socket_nmea_address) self.client_socket_warning_address = False self.client_socket = self.new_socket_connection( connection, self.client_socket_nmea_address) self.connecting_client_socket = False def nmea_process(self): print('nmea process', os.getpid()) self.setup() while True: t0 = time.monotonic() timeout = 100 if self.sockets else 10000 self.poll(timeout) def receive_pipe(self): while True: # receive all messages in pipe msg = self.pipe.recv() if not msg: return if msg[0] != '$': # perform checksum in this subprocess msg = '$' + msg + ('*%02X' % nmea_cksum(msg)) # relay nmea message from server to all tcp sockets for sock in self.sockets: sock.write(msg + '\r\n') def poll(self, timeout=0): t0 = time.monotonic() events = self.poller.poll(timeout) t1 = time.monotonic() if t1 - t0 > timeout: print(_('poll took too long in nmea process!')) while events: fd, flag = events.pop() sock = self.fd_to_socket[fd] if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL): if sock == self.server: print(_('nmea bridge lost server connection')) exit(2) if sock == self.pipe: print(_('nmea bridge lost pipe to autopilot')) exit(2) self.socket_lost(sock, fd) elif sock == self.server: self.new_socket_connection(*self.server.accept()) elif sock == self.pipe: self.receive_pipe() elif sock == self.client: pass # wake from poll elif sock == self.connecting_client_socket and flag & select.POLLOUT: self.poller.unregister(fd) del self.fd_to_socket[fd] self.client_connected(self.connecting_client_socket) elif flag & select.POLLIN: if not sock.recvdata(): self.socket_lost(sock, fd) else: while True: line = sock.readline() if not line: break self.receive_nmea(line, sock) else: print(_('nmea bridge unhandled poll flag'), flag) # if we are not multiprocessing, the pipe won't be pollable, so receive any data now if not self.process: self.receive_pipe() t2 = time.monotonic() # send any parsed nmea messages the server might care about if self.msgs: #print('nmea msgs', self.msgs) if self.pipe.send(self.msgs): self.msgs = {} t3 = time.monotonic() # receive pypilot messages pypilot_msgs = self.client.receive() for name in pypilot_msgs: value = pypilot_msgs[name] self.last_values[name] = value #except Exception as e: # print('nmea exception receiving:', e) t4 = time.monotonic() # flush sockets for sock in self.sockets: sock.flush() t5 = time.monotonic() # reconnect client tcp socket self.connect_client() t6 = time.monotonic() if t6 - t1 > .1: print(_('nmea process loop too slow:'), t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4, t6 - t5)