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)
