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 __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 __init__(self):
# split pipe ends
self.pipe, pipe = NonBlockingPipe('gps_pipe', True)
super(gpsProcess, self).__init__(target=self.gps_process,
args=(pipe, ),
daemon=True)
self.devices = []
def __init__(self):
# split pipe ends
self.gpsd_failed_connect = False
self.pipe, pipe = NonBlockingPipe('gps pipe', True)
super(gpsProcess, self).__init__(target=self.gps_process,
args=(pipe, ),
daemon=True)
def pipe(self):
if self.initialized:
print('direct pipe clients must be created before the server is run')
exit(0)
pipe0, pipe1 = NonBlockingPipe('pypilotServer pipe' + str(len(self.pipes)), self.multiprocessing)
self.pipes.append(pipe1)
return pipe0
def __init__(self, server):
self.client = pypilotClient(server)
self.multiprocessing = server.multiprocessing
if self.multiprocessing:
self.pipe, pipe = NonBlockingPipe('imu pipe', self.multiprocessing)
self.process = multiprocessing.Process(target=self.process, args=(pipe,), daemon=True)
self.process.start()
return
self.process = False
self.setup()
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 __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, cal_pipe = NonBlockingPipe('cal pipe',
True,
sendfailok=True)
else:
self.cal_pipe, cal_pipe = False, False # use client
client = pypilotClient(server)
super(AutomaticCalibrationProcess,
self).__init__(target=CalibrationProcess,
args=(cal_pipe, client),
daemon=True)
self.start()
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 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 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.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)
