class signalk(object): def __init__(self, sensors=False): self.sensors = sensors if not sensors: # only signalk process for testing self.client = pypilotClient() self.multiprocessing = False else: server = sensors.client.server self.multiprocessing = server.multiprocessing self.client = pypilotClient(server) self.initialized = False self.missingzeroconfwarned = False self.signalk_access_url = False self.last_access_request_time = 0 self.sensors_pipe, self.sensors_pipe_out = NonBlockingPipe('signalk pipe', self.multiprocessing) if self.multiprocessing: import multiprocessing self.process = multiprocessing.Process(target=self.process, daemon=True) self.process.start() else: self.process = False def setup(self): try: f = open(token_path) self.token = f.read() print('signalk' + _('read token'), self.token) f.close() except Exception as e: print('signalk ' + _('failed to read token'), token_path) self.token = False try: from zeroconf import ServiceBrowser, ServiceStateChange, Zeroconf except Exception as e: if not self.missingzeroconfwarned: print('signalk: ' + _('failed to') + ' import zeroconf, ' + _('autodetection not possible')) print(_('try') + ' pip3 install zeroconf' + _('or') + ' apt install python3-zeroconf') self.missingzeroconfwarned = True time.sleep(20) return self.last_values = {} self.last_sources = {} self.signalk_last_msg_time = {} # store certain values across parsing invocations to ensure # all of the keys are filled with the latest data self.last_values_keys = {} for sensor in signalk_table: for signalk_path_conversion, pypilot_path in signalk_table[sensor].items(): signalk_path, signalk_conversion = signalk_path_conversion if type(pypilot_path) == type({}): # single path translates to multiple pypilot self.last_values_keys[signalk_path] = {} self.period = self.client.register(RangeProperty('signalk.period', .5, .1, 2, persistent=True)) self.uid = self.client.register(Property('signalk.uid', 'pypilot', persistent=True)) self.signalk_host_port = False self.signalk_ws_url = False self.ws = False class Listener: def __init__(self, signalk): self.signalk = signalk self.name_type = False def remove_service(self, zeroconf, type, name): print('signalk zeroconf ' + _('service removed'), name, type) if self.name_type == (name, type): self.signalk.signalk_host_port = False self.signalk.disconnect_signalk() print('signalk ' + _('server lost')) def update_service(self, zeroconf, type, name): self.add_service(zeroconf, type, name) def add_service(self, zeroconf, type, name): print('signalk zeroconf ' + _('service add'), name, type) self.name_type = name, type info = zeroconf.get_service_info(type, name) if not info: return properties = {} for name, value in info.properties.items(): try: properties[name.decode()] = value.decode() except Exception as e: print('signalk zeroconf exception', e, name, value) if 'swname' in properties and properties['swname'] == 'signalk-server': try: host_port = socket.inet_ntoa(info.addresses[0]) + ':' + str(info.port) except Exception as e: host_port = socket.inet_ntoa(info.address) + ':' + str(info.port) self.signalk.signalk_host_port = host_port print('signalk ' + _('server found'), host_port) zeroconf = Zeroconf() listener = Listener(self) browser = ServiceBrowser(zeroconf, "_http._tcp.local.", listener) #zeroconf.close() self.initialized = True def probe_signalk(self): print('signalk ' + _('probe') + '...', self.signalk_host_port) try: import requests except Exception as e: print('signalk ' + _('could not') + ' import requests', e) print(_('try') + " 'sudo apt install python3-requests' " + _('or') + " 'pip3 install requests'") time.sleep(50) return try: r = requests.get('http://' + self.signalk_host_port + '/signalk') contents = pyjson.loads(r.content) self.signalk_ws_url = contents['endpoints']['v1']['signalk-ws'] + '?subscribe=none' except Exception as e: print(_('failed to retrieve/parse data from'), self.signalk_host_port, e) time.sleep(5) self.signalk_host_port = False return print('signalk ' + _('found'), self.signalk_ws_url) def request_access(self): import requests if self.signalk_access_url: dt = time.monotonic() - self.last_access_request_time if dt < 10: return self.last_access_request_time = time.monotonic() try: r = requests.get(self.signalk_access_url) contents = pyjson.loads(r.content) print('signalk ' + _('see if token is ready'), self.signalk_access_url, contents) if contents['state'] == 'COMPLETED': if 'accessRequest' in contents: access = contents['accessRequest'] if access['permission'] == 'APPROVED': self.token = access['token'] print('signalk ' + _('received token'), self.token) try: f = open(token_path, 'w') f.write(self.token) f.close() except Exception as e: print('signalk ' + _('failed to store token'), token_path) # if permission == DENIED should we try other servers?? self.signalk_access_url = False except Exception as e: print('signalk ' + _('error requesting access'), e) self.signalk_access_url = False return try: def random_number_string(n): if n == 0: return '' import random return str(int(random.random()*10)) + random_number_string(n-1) if self.uid.value == 'pypilot': self.uid.set('pypilot-' + random_number_string(11)) r = requests.post('http://' + self.signalk_host_port + '/signalk/v1/access/requests', data={"clientId":self.uid.value, "description": "pypilot"}) contents = pyjson.loads(r.content) print('signalk post', contents) if contents['statusCode'] == 202 or contents['statusCode'] == 400: self.signalk_access_url = 'http://' + self.signalk_host_port + contents['href'] print('signalk ' + _('request access url'), self.signalk_access_url) except Exception as e: print('signalk ' + _('error requesting access'), e) self.signalk_ws_url = False def connect_signalk(self): try: from websocket import create_connection, WebSocketBadStatusException except Exception as e: print('signalk ' + _('cannot create connection:'), e) print(_('try') + ' pip3 install websocket-client ' + _('or') + ' apt install python3-websocket') self.signalk_host_port = False return self.subscribed = {} for sensor in list(signalk_table): self.subscribed[sensor] = False self.subscriptions = [] # track signalk subscriptions self.signalk_values = {} self.keep_token = False try: self.ws = create_connection(self.signalk_ws_url, header={'Authorization': 'JWT ' + self.token}) self.ws.settimeout(0) # nonblocking except WebSocketBadStatusException: print('signalk ' + _('bad status, rejecting token')) self.token = False self.ws = False except ConnectionRefusedError: print('signalk ' + _('connection refused')) #self.signalk_host_port = False self.signalk_ws_url = False time.sleep(5) except Exception as e: print('signalk ' + _('failed to connect'), e) self.signalk_ws_url = False time.sleep(5) def process(self): time.sleep(6) # let other stuff load print('signalk process', os.getpid()) self.process = False while True: time.sleep(.1) self.poll(1) def poll(self, timeout=0): if self.process: msg = self.sensors_pipe_out.recv() while msg: sensor, data = msg self.sensors.write(sensor, data, 'signalk') msg = self.sensors_pipe_out.recv() return t0 = time.monotonic() if not self.initialized: self.setup() return self.client.poll(timeout) if not self.signalk_host_port: return # waiting for signalk to detect t1 = time.monotonic() if not self.signalk_ws_url: self.probe_signalk() return t2 = time.monotonic() if not self.token: self.request_access() return t3 = time.monotonic() if not self.ws: self.connect_signalk() if not self.ws: return print('signalk ' + _('connected to'), self.signalk_ws_url) # setup pypilot watches watches = ['imu.heading_lowpass', 'imu.roll', 'imu.pitch', 'timestamp'] for watch in watches: self.client.watch(watch, self.period.value) for sensor in signalk_table: self.client.watch(sensor+'.source') return # at this point we have a connection # read all messages from pypilot while True: msg = self.client.receive_single() if not msg: break debug('signalk pypilot msg', msg) name, value = msg if name == 'timestamp': self.send_signalk() self.last_values = {} if name.endswith('.source'): # update sources for sensor in signalk_table: source_name = sensor + '.source' if name == source_name: self.update_sensor_source(sensor, value) self.last_sources[name[:-7]] = value else: self.last_values[name] = value t4 = time.monotonic() while True: try: msg = self.ws.recv() except Exception as e: break if not msg: print('signalk server closed connection') if not self.keep_token: print('signalk invalidating token') self.token = False self.disconnect_signalk() return try: self.receive_signalk(msg) except Exception as e: debug('failed to parse signalk', e) return self.keep_token = True # do not throw away token if we got valid data t5 = time.monotonic() # convert received signalk values into sensor inputs if possible for sensor, sensor_table in signalk_table.items(): for source, values in self.signalk_values.items(): data = {} for signalk_path_conversion, pypilot_path in sensor_table.items(): signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in values: try: if not 'timestamp'in data and signalk_path in self.signalk_last_msg_time: ts = time.strptime(self.signalk_last_msg_time[signalk_path], '%Y-%m-%dT%H:%M:%S.%f%z') data['timestamp'] = time.mktime(ts) value = values[signalk_path] if type(pypilot_path) == type({}): # single path translates to multiple pypilot for signalk_key, pypilot_key in pypilot_path.items(): data[pypilot_key] = value[signalk_key] / signalk_conversion else: data[pypilot_path] = value / signalk_conversion except Exception as e: print(_('Exception converting signalk->pypilot'), e, self.signalk_values) break elif signalk_conversion != 1: # don't require fields with conversion of 1 break # missing fields? skip input this iteration else: for signalk_path_conversion in sensor_table: signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in values: del values[signalk_path] # all needed sensor data is found data['device'] = source + 'signalk' if self.sensors_pipe: self.sensors_pipe.send([sensor, data]) else: debug('signalk ' + _('received'), sensor, data) break #print('sigktimes', t1-t0, t2-t1, t3-t2, t4-t3, t5-t4) def send_signalk(self): # see if we can produce any signalk output from the data we have read updates = [] for sensor in signalk_table: if sensor != 'imu' and (not sensor in self.last_sources or\ source_priority[self.last_sources[sensor]]>=signalk_priority): #debug('signalk skip send from priority', sensor) continue for signalk_path_conversion, pypilot_path in signalk_table[sensor].items(): signalk_path, signalk_conversion = signalk_path_conversion if type(pypilot_path) == type({}): # single path translates to multiple pypilot keys = self.last_values_keys[signalk_path] # store keys we need for this signalk path in dictionary for signalk_key, pypilot_key in pypilot_path.items(): key = sensor+'.'+pypilot_key if key in self.last_values: keys[key] = self.last_values[key] # see if we have the keys needed v = {} for signalk_key, pypilot_key in pypilot_path.items(): key = sensor+'.'+pypilot_key if not key in keys: break v[signalk_key] = keys[key]*signalk_conversion else: updates.append({'path': signalk_path, 'value': v}) self.last_values_keys[signalk_path] = {} else: key = sensor+'.'+pypilot_path if key in self.last_values: v = self.last_values[key]*signalk_conversion updates.append({'path': signalk_path, 'value': v}) if updates: # send signalk updates msg = {'updates':[{'$source':'pypilot','values':updates}]} debug('signalk updates', msg) try: self.ws.send(pyjson.dumps(msg)+'\n') except Exception as e: print('signalk ' + _('failed to send updates'), e) self.disconnect_signalk() def disconnect_signalk(self): if self.ws: self.ws.close() self.ws = False self.client.clear_watches() # don't need to receive pypilot data def receive_signalk(self, msg): try: data = pyjson.loads(msg) except: if msg: print('signalk ' + _('failed to parse msg:'), msg) return if 'updates' in data: updates = data['updates'] for update in updates: source = 'unknown' if 'source' in update: source = update['source']['talker'] elif '$source' in update: source = update['$source'] if 'timestamp' in update: timestamp = update['timestamp'] if not source in self.signalk_values: self.signalk_values[source] = {} if 'values' in update: values = update['values'] elif 'meta' in update: values = update['meta'] else: debug('signalk message update contains no values or meta', update) continue for value in values: path = value['path'] if path in self.signalk_last_msg_time: if self.signalk_last_msg_time[path] == timestamp: debug('signalk skip duplicate timestamp', source, path, timestamp) continue self.signalk_values[source][path] = value['value'] else: debug('signalk skip initial message', source, path, timestamp) self.signalk_last_msg_time[path] = timestamp def update_sensor_source(self, sensor, source): priority = source_priority[source] watch = priority < signalk_priority # translate from pypilot -> signalk if watch: watch = self.period.value for signalk_path_conversion, pypilot_path in signalk_table[sensor].items(): if type(pypilot_path) == type({}): for signalk_key, pypilot_key in pypilot_path.items(): pypilot_path = sensor + '.' + pypilot_key if pypilot_path in self.last_values: del self.last_values[pypilot_path] self.client.watch(pypilot_path, watch) else: # remove any last values from this sensor pypilot_path = sensor + '.' + pypilot_path if pypilot_path in self.last_values: del self.last_values[pypilot_path] self.client.watch(pypilot_path, watch) subscribe = priority >= signalk_priority # prevent duplicating subscriptions if self.subscribed[sensor] == subscribe: return self.subscribed[sensor] = subscribe if not subscribe: #signalk can't unsubscribe by path!?!?! subscription = {'context': '*', 'unsubscribe': [{'path': '*'}]} debug('signalk unsubscribe', subscription) try: self.ws.send(pyjson.dumps(subscription)+'\n') except Exception as e: print('signalk failed to send', e) self.disconnect_signalk() return signalk_sensor = signalk_table[sensor] if subscribe: # translate from signalk -> pypilot subscriptions = [] for signalk_path_conversion in signalk_sensor: signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in self.signalk_last_msg_time: del self.signalk_last_msg_time[signalk_path] subscriptions.append({'path': signalk_path, 'minPeriod': self.period.value*1000, 'format': 'delta', 'policy': 'instant'}) self.subscriptions += subscriptions else: # remove this subscription and resend all subscriptions debug('signalk remove subs', signalk_sensor, self.subscriptions) subscriptions = [] for subscription in self.subscriptions: for signalk_path_conversion in signalk_sensor: signalk_path, signalk_conversion = signalk_path_conversion if subscription['path'] == signalk_path: break else: subscriptions.append(subscription) self.subscriptions = subscriptions self.signalk_last_msg_time = {} subscription = {'context': 'vessels.self'} subscription['subscribe'] = subscriptions debug('signalk subscribe', subscription) try: self.ws.send(pyjson.dumps(subscription)+'\n') except Exception as e: print('signalk failed to send subscription', e) self.disconnect_signalk()
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 signalk(object): def __init__(self, sensors=False): self.sensors = sensors if not sensors: # only signalk process for testing self.client = pypilotClient() self.multiprocessing = False else: server = sensors.client.server self.multiprocessing = server.multiprocessing self.client = pypilotClient(server) self.initialized = False self.missingzeroconfwarned = False self.signalk_access_url = False self.last_access_request_time = 0 self.sensors_pipe, self.sensors_pipe_out = NonBlockingPipe( 'nmea pipe', self.multiprocessing) if self.multiprocessing: import multiprocessing self.process = multiprocessing.Process(target=self.process, daemon=True) self.process.start() else: self.process = False def setup(self): try: f = open(token_path) self.token = f.read() print('read token', self.token) f.close() except Exception as e: print('signalk failed to read token', token_path) self.token = False try: from zeroconf import ServiceBrowser, ServiceStateChange, Zeroconf except Exception as e: if not self.missingzeroconfwarned: print( 'signalk: failed to import zeroconf, autodetection not possible' ) print( 'try pip3 install zeroconf or apt install python3-zeroconf' ) self.missingzeroconfwarned = True time.sleep(20) return self.last_values = {} self.signalk_msgs = {} self.signalk_msgs_skip = {} self.period = self.client.register( RangeProperty('signalk.period', .5, .1, 2, persistent=True)) self.signalk_host_port = False self.signalk_ws_url = False self.ws = False class Listener: def __init__(self, signalk): self.signalk = signalk self.name_type = False def remove_service(self, zeroconf, type, name): print('zeroconf service removed', name, type) if self.name_type == (name, type): self.signalk.signalk_host_port = False self.signalk.disconnect_signalk() print('signalk server lost') def add_service(self, zeroconf, type, name): print('zeroconf service add', name, type) self.name_type = name, type info = zeroconf.get_service_info(type, name) if not info: return properties = {} for name, value in info.properties.items(): properties[name.decode()] = value.decode() if properties['swname'] == 'signalk-server': try: host_port = socket.inet_ntoa( info.addresses[0]) + ':' + str(info.port) except Exception as e: host_port = socket.inet_ntoa(info.address) + ':' + str( info.port) self.signalk.signalk_host_port = host_port print('signalk server found', host_port) zeroconf = Zeroconf() listener = Listener(self) browser = ServiceBrowser(zeroconf, "_http._tcp.local.", listener) #zeroconf.close() self.initialized = True def probe_signalk(self): print('signalk probe...', self.signalk_host_port) try: import requests except Exception as e: print('signalk could not import requests', e) print( "try 'sudo apt install python3-requests' or 'pip3 install requests'" ) time.sleep(50) return try: r = requests.get('http://' + self.signalk_host_port + '/signalk') contents = pyjson.loads(r.content) self.signalk_ws_url = contents['endpoints']['v1'][ 'signalk-ws'] + '?subscribe=none' except Exception as e: print('failed to retrieve/parse data from', self.signalk_host_port, e) time.sleep(5) return print('signalk found', self.signalk_ws_url) def request_access(self): import requests if self.signalk_access_url: dt = time.monotonic() - self.last_access_request_time if dt < 10: return self.last_access_request_time = time.monotonic() try: r = requests.get(self.signalk_access_url) contents = pyjson.loads(r.content) print('signalk see if token is ready', self.signalk_access_url, contents) if contents['state'] == 'COMPLETED': if 'accessRequest' in contents: access = contents['accessRequest'] if access['permission'] == 'APPROVED': self.token = access['token'] print('signalk received token', self.token) try: f = open(token_path, 'w') f.write(self.token) f.close() except Exception as e: print('signalk failed to store token', token_path) else: self.signalk_access_url = False except Exception as e: print('error requesting access', e) self.signalk_access_url = False return try: uid = "1234-45653343454" r = requests.post('http://' + self.signalk_host_port + '/signalk/v1/access/requests', data={ "clientId": uid, "description": "pypilot" }) contents = pyjson.loads(r.content) print('post', contents) if contents['statusCode'] == 202 or contents['statusCode'] == 400: self.signalk_access_url = 'http://' + self.signalk_host_port + contents[ 'href'] print('signalk request access url', self.signalk_access_url) except Exception as e: print('signalk error requesting access', e) self.signalk_ws_url = False def connect_signalk(self): try: from websocket import create_connection except Exception as e: print('signalk cannot create connection:', e) print( 'try pip3 install websocket-client or apt install python3-websocket' ) self.signalk_host_port = False return self.subscribed = {} for sensor in list(signalk_table): self.subscribed[sensor] = False self.subscriptions = [] # track signalk subscriptions self.signalk_values = {} try: self.ws = create_connection( self.signalk_ws_url, header={'Authorization': 'JWT ' + self.token}) self.ws.settimeout(0) # nonblocking except Exception as e: print('failed to connect signalk', e) self.token = False def process(self): time.sleep(6) # let other stuff load print('signalk process', os.getpid()) self.process = False while True: time.sleep(.1) self.poll(1) def poll(self, timeout=0): if self.process: msg = self.sensors_pipe_out.recv() while msg: sensor, data = msg self.sensors.write(sensor, data, 'signalk') msg = self.sensors_pipe_out.recv() return t0 = time.monotonic() if not self.initialized: self.setup() return self.client.poll(timeout) if not self.signalk_host_port: return # waiting for signalk to detect t1 = time.monotonic() if not self.signalk_ws_url: self.probe_signalk() return t2 = time.monotonic() if not self.token: self.request_access() return t3 = time.monotonic() if not self.ws: self.connect_signalk() if not self.ws: return print('connected to signalk server') # setup pypilot watches watches = [ 'imu.heading_lowpass', 'imu.roll', 'imu.pitch', 'timestamp' ] for watch in watches: self.client.watch(watch, self.period.value) for sensor in signalk_table: self.client.watch(sensor + '.source') return # at this point we have a connection # read all messages from pypilot while True: msg = self.client.receive_single() if not msg: break name, value = msg if name == 'timestamp': self.send_signalk() self.last_values = {} # reset last values self.signalk_msgs = {} self.last_values[name] = value if name.endswith('.source'): # update sources for sensor in signalk_table: source_name = sensor + '.source' if name == source_name: self.update_sensor_source(sensor, value) t4 = time.monotonic() while True: try: msg = self.ws.recv() print('sigk', msg) except: break self.receive_signalk(msg) t5 = time.monotonic() for sensor, sensor_table in signalk_table.items(): for source, values in self.signalk_values.items(): data = {} for signalk_path_conversion, pypilot_path in sensor_table.items( ): signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in values: data[pypilot_path] = values[ signalk_path] / signalk_conversion elif signalk_conversion != 1: # don't require fields with conversion of 1 (lat/lon) break else: for signalk_path in sensor_table: if signalk_path in values: del values[signalk_path] # all needed sensor data is found data['device'] = source print('signalk data', data, sensor) if self.sensors_pipe: self.sensors_pipe.send([sensor, data]) else: print('signalk received', sensor, data) break #print('sigktimes', t1-t0, t2-t1, t3-t2, t4-t3, t5-t4) def send_signalk(self): # see if we can produce any signalk output from the data we have read updates = [] for sensor in signalk_table: for signalk_path_conversion, pypilot_path in signalk_table[ sensor].items(): signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in self.signalk_msgs: continue if type(pypilot_path) == type( {}): # single path translates to multiple pypilot v = {} for signalk_key, pypilot_key in pypilot_path.items(): key = sensor + '.' + pypilot_key if not key in self.last_values: break v[signalk_key] = self.last_values[ key] * signalk_conversion else: updates.append({'path': signalk_path, 'value': v}) self.signalk_msgs[signalk_path] = True else: key = sensor + '.' + pypilot_path if key in self.last_values: v = self.last_values[key] * signalk_conversion updates.append({'path': signalk_path, 'value': v}) self.signalk_msgs[signalk_path] = True if updates: # send signalk updates msg = {'updates': [{'$source': 'pypilot', 'values': updates}]} #print('signalk updates', msg) try: self.ws.send(pyjson.dumps(msg) + '\n') except Exception as e: print('signalk failed to send', e) self.disconnect_signalk() def disconnect_signalk(self): if self.ws: self.ws.close() self.ws = False self.client.clear_watches() # don't need to receive pypilot data def receive_signalk(self, msg): try: data = pyjson.loads(msg) except: print('failed to parse signalk msg:', msg) return if 'updates' in data: updates = data['updates'] for update in updates: source = 'unknown' if 'source' in update: source = update['source']['talker'] elif '$source' in update: source = update['$source'] if 'timestamp' in update: timestamp = update['timestamp'] if not source in self.signalk_values: self.signalk_values[source] = {} for value in update['values']: path = value['path'] if path in self.signalk_msgs_skip: self.signalk_values[source][path] = value['value'] else: self.signalk_msgs_skip[path] = True def update_sensor_source(self, sensor, source): priority = source_priority[source] sk_priority = source_priority['signalk'] watch = priority < sk_priority # translate from pypilot -> signalk if watch: watch = self.period.value for signalk_path_conversion, pypilot_path in signalk_table[ sensor].items(): if type(pypilot_path) == type({}): for signalk_key, pypilot_key in pypilot_path.items(): self.client.watch(sensor + '.' + pypilot_key, watch) else: self.client.watch(sensor + '.' + pypilot_path, watch) subscribe = priority >= sk_priority # prevent duplicating subscriptions if self.subscribed[sensor] == subscribe: return self.subscribed[sensor] = subscribe if not subscribe: #signalk can't unsubscribe by path!?!?! subscription = {'context': '*', 'unsubscribe': [{'path': '*'}]} self.ws.send(pyjson.dumps(subscription) + '\n') signalk_sensor = signalk_table[sensor] if subscribe: subscriptions = [] for signalk_path_conversion in signalk_sensor: signalk_path, signalk_conversion = signalk_path_conversion if signalk_path in self.signalk_msgs_skip: del self.signalk_msgs_skip[signalk_path] subscriptions.append({ 'path': signalk_path, 'minPeriod': self.period.value * 1000, 'format': 'delta', 'policy': 'instant' }) self.subscriptions += subscriptions else: # remove this subscription and resend all subscriptions subscriptions = [] for subscription in self.subscriptions: for signalk_path in signalk_sensor: if subscription['path'] == signalk_path: break else: subscriptions.append(subscription) self.subscriptions = subscriptions self.signalk_msgs_skip = {} subscription = {'context': 'vessels.self'} subscription['subscribe'] = subscriptions #print('signalk subscribe', subscription) self.ws.send(pyjson.dumps(subscription) + '\n')
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)