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 LearningPilot(AutopilotPilot): def __init__(self, ap): super(LearningPilot, self).__init__('learning', ap) # create filters timestamp = self.ap.server.TimeStamp('ap') # create simple pid filter self.P = self.Register(AutopilotGain, 'P', .001, .0001, .01) self.D = self.Register(AutopilotGain, 'D', .03, .01, .1) self.lag = self.Register(RangeProperty, 'lag', 1, 0, 5) timestamp = self.ap.server.TimeStamp('ap') self.dt = self.Register(SensorValue, 'dt', timestamp) self.initialized = False self.start_time = time.time() self.model_uid = 0 def reset(self): PreTrain(self.model) def initialize(self): # Build model self.history = History() self.learning_pipe, pipe = NonBlockingPipe('learning_pipe') model_pipe, self.model_pipe = NonBlockingPipe('learning_model_pipe') self.model_pipe_poller = select.poll() self.model_pipe_poller.register(pipe, select.POLLIN) self.fit_process = multiprocessing.Process(target=LearningProcess, args=(pipe, model_pipe)) self.fit_process.start() print('start training') self.initialized = True def process(self, reset): ap = self.ap if not self.initialized: if time.time() - self.start_time < 2: return self.initialize() P = ap.heading_error.value D = ap.boatimu.SensorValues['headingrate_lowpass'].value accel = ap.boatimu.SensorValues['accel'].value gyro = ap.boatimu.SensorValues['gyro'].value wind = ap.sensors.wind # input data data = [P, D] + list(accel) + list(gyro) data += [ap.servo.voltage.value / 24, ap.servo.current.value / 20] data += [wind.direction.value / 360, wind.speed.value / 60] # training data lag_samples = int(self.lag.value / self.ap.boatimu.rate.value) self.history.put(data, samples + lag_samples) learning_history = self.history.data[lag_samples:] if len(learning_history) == samples: #error = 0 #for d in self.history.data[:lag_samples]: # error += d[0]*self.P.value + d[1]*self.D.value # calculate error from current state #error /= lag_samples d = self.history.data[0] e = d[0] * self.P.value + d[ 1] * self.D.value # calculate error from current state # see what our command was to find the better command data = { 'input': learning_history[0], 'error': e, 'uid': self.model_uid } self.learning_pipe.send(data) history = self.history.data[:samples] if len(history) == samples: if self.model_pipe_poller.poll(): tflite_model, self.model_uid = self.model_pipe.recv() open('converted.tflite', 'wb').write(tflite_model) if self.model_uid: t0 = time.time() interpreter = tf.lite.Interpreter( model_path="converted.tflite") t1 = time.time() interpreter.allocate_tensors() t2 = time.time() input_details = interpreter.get_input_details() output_details = interpreter.get_output_details() t3 = time.time() input_shape = input_details[0]['shape'] print('input details', input_details) t4 = time.time() interpreter.set_tensor(input_details[0]['index'], np.array(history)) interpreter.invoke() t5 = time.time() output_data = interpreter.get_tensor( output_details[0]['index']) t6 = time.time() print('interpreter timings', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4, t6 - t5) if ap.enabled.value and len(self.history.data) >= samples: ap.servo.command.set(command)
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)
output_details[0]['index']) t6 = time.time() print('interpreter timings', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4, t6 - t5) if ap.enabled.value and len(self.history.data) >= samples: ap.servo.command.set(command) pilot = LearningPilot if __name__ == '__main__': learning_pipe, pipe = NonBlockingPipe('learning_pipe') fit_process = multiprocessing.Process(target=LearningProcess, args=(pipe, )) fit_process.start() x = 0 while True: P = math.sin(x) D = math.sin(x + 3) x += .01 inp = [0] * num_inputs inp[0] = P inp[1] = D error = math.sin(x - 1) data = {'input': inp, 'error': error} learning_pipe.send(data) time.sleep(.1)