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)
