def work_pypilot():
#init compass
mode = conf.get('PYPILOT', 'mode')
if mode == 'disabled':
print 'pypilot disabled '
return
headingSK = conf.get('PYPILOT', 'translation_magnetic_h')
attitudeSK = conf.get('PYPILOT', 'translation_attitude')
SETTINGS_FILE = "RTIMULib"
s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)
imuName = imu.IMUName()
del imu
del s
if mode == 'imu':
cmd = ['pypilot_boatimu', '-q']
elif mode == 'basic autopilot':
# ensure no serial getty running
os.system('sudo systemctl stop [email protected]')
os.system('sudo systemctl stop [email protected]')
cmd = ['pypilot']
try:
translation_rate = float(conf.get('PYPILOT', 'translation_rate'))
except:
translation_rate = 1
conf.set('PYPILOT', 'translation_rate', '1')
pid = os.fork()
try:
if pid == 0:
os.execvp(cmd[0], cmd)
print 'failed to launch', cmd
exit(1)
except:
print 'exception launching pypilot'
exit(1)
print 'launched pypilot pid', pid
time.sleep(3) # wait 3 seconds to launch client
def on_con(client):
print 'connected'
if headingSK == '1':
client.watch('imu.heading')
if attitudeSK == '1':
client.watch('imu.pitch')
client.watch('imu.roll')
client = False
tick1 = time.time()
while read_sensors:
ret = os.waitpid(pid, os.WNOHANG)
if ret[0] == pid:
# should we respawn pypilot if it crashes?
print 'pypilot exited'
break
# connect to pypilot if not connected
try:
if not client:
client = SignalKClient(on_con, 'localhost')
except:
time.sleep(1)
continue # not much to do without connection
try:
result = client.receive()
except:
print 'disconnected from pypilot'
client = False
continue
Erg = Translate(result)
SignalK='{"updates":[{"$source":"OPsensors.I2C.'+imuName+'","values":['
SignalK+=Erg[0:-1]+'}]}]}\n'
sock.sendto(SignalK, ('127.0.0.1', 55557))
if mode == 'imu':
if 'imu.heading' in result:
value = result['imu.heading']['value']
hdm = str(pynmea2.HDM('AP', 'HDM', (str(value),'M')))+'\r\n'
sock.sendto(hdm, ('127.0.0.1', 10110))
if 'imu.roll' in result:
value = result['imu.roll']['value']
xdr_r = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','ROLL')))+'\r\n'
sock.sendto(xdr_r, ('127.0.0.1', 10110))
if 'imu.pitch' in result:
value = result['imu.pitch']['value']
xdr_p = str(pynmea2.XDR('AP', 'XDR', ('A',str(value),'D','PTCH')))+'\r\n'
sock.sendto(xdr_p, ('127.0.0.1', 10110))
while True:
dt = translation_rate - time.time() + tick1
if dt <= 0:
break
time.sleep(dt)
tick1 = time.time()
# cleanup
print 'stopping pypilot pid:', pid
try:
os.kill(pid, 15)
time.sleep(1) # wait one second to shut down pypilot
except Exception, e:
print 'exception stopping pypilot', e
class MainFrame(wx.Frame):
def __init__(self):
wx.Frame.__init__(self, None, title="signalk client", size=(1000, 600))
self.value_list = []
self.client = SignalKClientFromArgs(sys.argv, True, self.on_con)
self.host_port = self.client.host_port
self.client.autoreconnect = False
ssizer = wx.FlexGridSizer(0, 1, 0, 0)
ssizer.AddGrowableRow(0)
ssizer.AddGrowableCol(0)
ssizer.SetFlexibleDirection(wx.BOTH)
ssizer.SetNonFlexibleGrowMode(wx.FLEX_GROWMODE_SPECIFIED)
self.scrolledWindow = wx.ScrolledWindow(self, wx.ID_ANY,
wx.DefaultPosition,
wx.DefaultSize,
wx.HSCROLL | wx.VSCROLL)
self.scrolledWindow.SetScrollRate(5, 5)
sizer = wx.FlexGridSizer(0, 3, 0, 0)
sizer.AddGrowableCol(2)
sizer.SetFlexibleDirection(wx.BOTH)
sizer.SetNonFlexibleGrowMode(wx.FLEX_GROWMODE_SPECIFIED)
self.values = {}
self.controls = {}
self.sliderrange = {}
self.value_list = self.client.list_values()
self.on_con(self.client)
for name in sorted(self.value_list):
sizer.Add(wx.StaticText(self.scrolledWindow, wx.ID_ANY, name), 0,
wx.ALL, 5)
self.values[name] = wx.StaticText(self.scrolledWindow, wx.ID_ANY)
sizer.Add(self.values[name], 0, wx.ALL, 5)
t = self.value_list[name]['type']
if t == 'Property':
tb = wx.TextCtrl(self.scrolledWindow, wx.ID_ANY)
sizer.Add(tb)
self.controls[name] = tb
elif t == 'BooleanProperty':
def proc(): # encapsulate to fix scope
cb = wx.CheckBox(self.scrolledWindow, wx.ID_ANY, '')
sizer.Add(cb, 0, wx.EXPAND)
self.controls[name] = cb
cbname = name
def oncheck(event):
self.client.set(cbname, cb.GetValue())
cb.Bind(wx.EVT_CHECKBOX, oncheck)
proc()
elif t == 'RangeProperty' or t == 'RangeSetting':
useSlider = True
def proc():
r = self.value_list[name]['min'], self.value_list[name][
'max']
if useSlider:
s = wx.Slider(self.scrolledWindow)
s.SetRange(0, 1000)
else:
s = wx.SpinCtrlDouble(self.scrolledWindow)
s.SetRange(r[0], r[1])
s.SetIncrement(min(1, (r[1] - r[0]) / 100.0))
s.SetDigits(-math.log(s.GetIncrement()) /
math.log(10) + 1)
sizer.Add(s, 0, wx.EXPAND)
self.controls[name] = s
sname = name
def onspin(event):
if useSlider:
v = s.GetValue() / 1000.0 * (r[1] - r[0]) + r[0]
self.client.set(sname, v)
else:
self.client.set(sname, s.GetValue())
if useSlider:
s.Bind(wx.EVT_SLIDER, onspin)
self.sliderrange[name] = r
else:
s.Bind(wx.EVT_SPINCTRLDOUBLE, onspin)
proc()
elif t == 'EnumProperty':
def proc():
c = wx.Choice(self.scrolledWindow, wx.ID_ANY)
for choice in self.value_list[name]['choices']:
c.Append(str(choice))
sizer.Add(c, 0, wx.EXPAND)
self.controls[name] = c
cname = name
def onchoice(event):
self.client.set(cname, str(c.GetStringSelection()))
c.Bind(wx.EVT_CHOICE, onchoice)
proc()
elif t == 'ResettableValue':
def proc():
b = wx.Button(self.scrolledWindow, wx.ID_ANY, 'Reset')
sizer.Add(b, 0, wx.EXPAND)
bname = name
def onclick(event):
self.client.set(bname, 0)
b.Bind(wx.EVT_BUTTON, onclick)
proc()
else:
sizer.Add(wx.StaticText(self.scrolledWindow, wx.ID_ANY, ''))
self.scrolledWindow.SetSizer(sizer)
self.scrolledWindow.Layout()
sizer.Fit(self.scrolledWindow)
ssizer.Add(self.scrolledWindow, 1, wx.EXPAND | wx.ALL, 5)
bsizer = wx.FlexGridSizer(1, 0, 0, 0)
self.bRefresh = wx.Button(self, wx.ID_ANY, 'Refresh')
self.bRefresh.Bind(wx.EVT_BUTTON, self.Refresh)
bsizer.Add(self.bRefresh)
self.bScope = wx.Button(self, wx.ID_ANY, 'Scope')
self.bScope.Bind(
wx.EVT_BUTTON, lambda event: subprocess.Popen([
'python',
os.path.abspath(os.path.dirname(__file__)) + '/' +
'scope_wx.py'
] + sys.argv[1:]))
bsizer.Add(self.bScope)
self.bClose = wx.Button(self, wx.ID_ANY, 'Close')
self.bClose.Bind(wx.EVT_BUTTON, exit)
bsizer.Add(self.bClose)
ssizer.Add(bsizer, 1, wx.EXPAND)
self.SetSizer(ssizer)
self.Layout()
self.timer = wx.Timer(self, wx.ID_ANY)
self.timer.Start(500)
self.Bind(wx.EVT_TIMER, self.receive_messages, id=wx.ID_ANY)
self.Refresh()
def Refresh(self):
for name in self.value_list:
self.client.get(name)
def on_con(self, client):
self.SetTitle("signalk client - Connected")
for name in sorted(self.value_list):
t = self.value_list[name]['type']
if t != 'SensorValue':
client.watch(name)
else:
client.get(name)
def receive_messages(self, event):
if not self.client:
try:
host, port = self.host_port
self.client = SignalKClient(self.on_con,
host,
port,
autoreconnect=False)
self.timer.Start(100)
except socket.error:
self.timer.Start(1000)
return
while True:
result = False
try:
result = self.client.receive()
except ConnectionLost:
self.SetTitle("signalk client - Disconnected")
self.client = False
return
except:
pass
if not result:
break
for name in result:
if not 'value' in result[name]:
print('no value', result)
raise 'no value'
value = round3(result[name]['value'])
strvalue = str(value)
if len(strvalue) > 50:
strvalue = strvalue[:47] + '...'
self.values[name].SetLabel(strvalue)
if name in self.controls:
try:
if str(type(self.controls[name])
) == "<class 'wx._controls.Choice'>":
if not self.controls[name].SetStringSelection(
value):
print(
'warning, invalid choice value specified')
elif str(type(self.controls[name])
) == "<class 'wx._controls.Slider'>":
r = self.sliderrange[name]
self.controls[name].SetValue(
float(value - r[0]) / (r[1] - r[0]) * 1000)
else:
self.controls[name].SetValue(value)
except:
self.controls[name].SetValue(str(value))
size = self.GetSize()
self.Fit()
self.SetSize(size)
class autogain(object):
def __init__(self):
self.search = [ \
#{'name': 'ap.I', 'min': 0, 'max': .006, 'step': .003},
#{'name': 'ap.P2', 'min': 0, 'max': .006, 'step': .006},
{'name': 'ap.P', 'min': .002, 'max': .006, 'step': .001},
{'name': 'ap.D', 'min': .06, 'max': .12, 'step': .01}]
self.variables = ['ap.heading_error', 'servo.Watts', 'servo.current', 'gps.speed']
self.settle_period = 2
self.period = 5
self.watchlist = ['ap.enabled']
for var in self.search:
self.watchlist.append(var['name'])
for var in self.variables:
self.watchlist.append(var)
def on_con(client):
for name in self.watchlist:
client.watch(name)
print 'connecting to server...'
host = False
if len(sys.argv) > 1:
host = sys.argv[1]
while True:
try:
self.client = SignalKClient(on_con, host, autoreconnect=True)
break
except:
time.sleep(2)
print 'connected'
def read_messages(self, log):
msgs = self.client.receive()
for name in msgs:
data = msgs[name]
value = data['value']
for var in self.search:
if name == var:
name = name[3:]
if abs(value - self.gains[name].value) > 1e-8:
print 'external program adjusting search variable!!, abrort', name, value
exit(0)
if log:
for var in self.variables:
if name == var:
self.total[name]['total'] += value
self.total[name]['count'] += 1
if name == 'ap.enabled' and not value:
#print 'autopilot disabled!!'
#exit(0)
pass
def set(self, name, val):
print 'setting', name, 'to', val
self.searchval[name] = val
self.client.set(name, val)
def log(self):
print 'logging for', self.searchval
t0 = time.time()
self.total = {}
for var in self.variables:
self.total[var] = {'total': 0, 'count': 0}
while time.time() - t0 < self.period:
self.read_messages(time.time() - t0 > self.settle_period)
time.sleep(.05)
for var in self.variables:
if not var in self.results:
self.results[var] = []
count = self.total[var]['count']
if count:
self.results[var].append((self.searchval.copy(), self.total[var]['total'] / count))
else:
print 'warning, no results for', var
def run_search(self, search):
if search:
s = search[0]
for val in frange(s['min'], s['max'], s['step']):
self.set(s['name'], val)
self.run_search(search[1:])
else:
self.log()
def result_range(self, results, name):
r = []
for result in results:
vars, val = result
r.append(vars[name])
return unique(sorted(r))
def result_value(self, results, vals):
values = []
for result in results:
vars, val = result
if vars == vals:
values.append(val)
if len(values) == 1:
return '%.3f' % values[0]
return values
def print_results(self, results, search, vals):
l = len(search)
if l < 2:
print 'error, need at least 2 search variables'
exit(1)
s = search[0]
if l > 2:
for val in self.result_range(results, s['name']):
print s['name'], '=', val
vals[s['name']] = val
self.print_results(results, search[1:], vals)
elif l == 2:
t = search[1]
print s['name'], '/', t['name']
line = '\t'
s_range = self.result_range(results, s['name'])
for val0 in s_range:
line += '%.3f\t' % val0
print line
for val1 in self.result_range(results, t['name']):
line = '%.3f\t' % val1
vals[t['name']] = val1
for val0 in s_range:
vals[s['name']] = val0
line += str(self.result_value(results, vals)) + '\t'
print line
print ''
def run(self):
self.searchval = {}
self.results = {}
self.run_search(self.search)
for var in self.variables:
print 'Results for', var
self.print_results(self.results[var], self.search, {})
print ''
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 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)
def nmea_bridge_process(pipe=False):
import os
sockets = []
watchlist = [
'ap.enabled', 'ap.mode', 'ap.heading_command', 'imu/pitch', 'imu/roll',
'imu/heading_lowpass', 'gps.source', 'wind.speed', 'wind.direction',
'wind.source'
]
def setup_watches(client, watch=True):
for name in watchlist:
client.watch(name, watch)
def on_con(client):
print 'nmea client connected'
if sockets:
setup_watches(client)
# we actually use a local connection to the server to simplify logic
print 'nmea try connections'
while True:
try:
client = SignalKClient(on_con, 'localhost', autoreconnect=True)
break
except:
time.sleep(2)
print 'nmea connected'
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setblocking(0)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
port = 10110
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)
max_connections = 10
READ_ONLY = select.POLLIN | select.POLLHUP | select.POLLERR
ap_enabled = 'N/A'
ap_mode = 'N/A'
ap_heading_command = 180
addresses = {}
cnt = 0
poller = select.poll()
poller.register(server, READ_ONLY)
fd_to_socket = {server.fileno(): server}
windspeed = 0
gps_source = wind_source = False
while True:
if sockets:
timeout = 100
else:
timeout = 10000
events = poller.poll(timeout)
while events:
event = events.pop()
fd, flag = event
sock = fd_to_socket[fd]
if sock == server:
connection, address = sock.accept()
if len(sockets) == max_connections:
connection.close()
else:
if not sockets:
setup_watches(client)
sock = LineBufferedNonBlockingSocket(connection)
sockets.append(sock)
print 'new connection: ', address
addresses[sock] = address
fd = sock.socket.fileno()
fd_to_socket[fd] = sock
poller.register(sock.socket, READ_ONLY)
elif (flag & (select.POLLHUP | select.POLLERR)) or \
(flag & select.POLLIN and not sock.recv()):
print 'lost connection: ', addresses[sock]
sockets.remove(sock)
# addresses.remove(sock)
if not sockets:
setup_watches(client, False)
poller.unregister(sock.socket)
fd = sock.socket.fileno()
del fd_to_socket[fd]
sock.socket.close()
# elif flag & select.POLLOUT:
# sock.flush()
# if not sock.out_buffer:
# poller.register(sock.socket, READ_ONLY)
for sock in sockets:
line = sock.readline()
if not line:
continue
if line[:6] == '$GPRMC':
if pipe and gps_source != 'internal':
data = line[7:len(line) - 3].split(',')
timestamp = float(data[0])
speed = float(data[6])
heading = float(data[7])
pipe.send(
{
'gps': {
'timestamp': timestamp,
'track': heading,
'speed': speed
}
}, False)
elif line[0] == '$' and line[3:6] == 'MVW':
if pipe and wind_source != 'internal':
winddata = wind.parse_nmea(line)
if winddata:
pipe.send({'wind': winddata}, False)
elif line[0] == '$' and line[3:6] == 'APB':
data = line[7:len(line) - 3].split(',')
if not ap_enabled:
client.set('ap.enabled', True)
if ap_mode != 'gps':
client.set('ap.mode', 'gps')
if abs(ap_heading_command - float(data[7])) > .1:
client.set('ap.heading_command', float(data[7]))
msgs = client.receive()
for name in msgs:
data = msgs[name]
value = data['value']
msg = False
if name == 'ap.enabled':
ap_enabled = value
elif name == 'ap.mode':
ap_mode = value
elif name == 'ap.heading_command':
ap_heading_command = value
elif name == 'imu/pitch':
msg = 'APXDR,A,%.3f,D,PTCH' % value
elif name == 'imu/roll':
msg = 'APXDR,A,%.3f,D,ROLL' % value
elif name == 'imu/heading_lowpass':
msg = 'APHDM,%.3f,M' % value
elif name == 'gps.source':
gps_source = value
elif name == 'wind.speed':
windspeed = value
elif name == 'wind.direction':
msg = 'APMWV,%.1f,R,%.1f,K,A' % (value, windspeed)
elif name == 'wind.source':
wind_source = value
if msg:
msg = '$' + msg + '*' + cksum(msg) + '\r\n'
for sock in sockets:
sock.send(msg)
sock.flush()