def connect(self):
self.display_page = self.display_connecting
watchlist = ['ap.enabled', 'ap.mode', 'ap.heading_command',
'gps.source', 'wind.source', 'servo.controller', 'servo.flags',
'imu.compass_calibration', 'imu.compass_calibration_sigmapoints',
'imu.compass_calibration_locked', 'imu.alignmentQ']
poll_list = ['ap.heading']
self.last_msg = {}
for name in ['gps.source', 'wind.source']:
self.last_msg[name] = 'none'
self.last_msg['ap.heading_command'] = 0
self.last_msg['imu.heading_offset'] = 0
host = ''
if len(sys.argv) > 1:
host = sys.argv[1]
def on_con(client):
self.value_list = {}
request = {'method' : 'list'}
client.send(request)
for name in watchlist:
client.watch(name)
try:
self.client = SignalKClient(on_con, host)
self.display_page = self.display_control
print 'connected'
for request in self.initial_gets:
self.get(request)
except:
self.client = False
time.sleep(1)
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 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
refresh = False
while True:
result = False
try:
result = self.client.receive_single()
except ConnectionLost:
self.client = False
return
except:
pass
if not result:
break
if self.watches[result[0]]:
if self.plot.read_data(result):
refresh = True
if refresh:
self.glArea.Refresh()
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)
def connect(self):
watchlist = [
'ap.enabled', 'ap.mode', 'ap.pilot', 'ap.bell_server',
'ap.heading', 'ap.heading_command', 'gps.source', 'wind.source',
'servo.controller', 'servo.flags', 'ap.pilot.basic.P',
'ap.pilot.basic.I', 'ap.pilot.basic.D'
]
self.last_msg = {}
host = ''
if len(sys.argv) > 1:
host = sys.argv[1]
def on_con(client):
self.value_list = client.list_values(10)
for name in watchlist:
client.watch(name)
try:
self.client = SignalKClient(on_con, host)
if self.value_list:
print('connected')
else:
client.disconnect()
raise 1
except Exception as e:
print(e)
self.client = False
time.sleep(1)
self.server = SignalKServer()
def Register(_type, name, *args, **kwargs):
return self.server.Register(_type(*([name] + list(args)),
**kwargs))
ap_bell_server = Register(
EnumProperty,
'ap.bell_server',
'10.10.10.1',
['10.10.10.1', '10.10.10.2', '10.10.10.4', '192.168.178.129'],
persistent=True)
self.last_msg['ap.bell_server'] = ap_bell_server.value
ap_pilot = Register(StringValue, 'ap.pilot', 'none')
self.last_msg['ap.pilot'] = ap_pilot.value
def receive_messages(self, event):
if not self.client:
try:
self.client = SignalKClient(self.on_con, self.host, autoreconnect=False)
except socket.error:
self.timer.Start(5000)
return
try:
msg = self.client.receive_single()
while msg:
self.receive_message(msg)
msg = self.client.receive_single()
self.timer.Start(50)
except ConnectionLost:
self.client = False
def connect(self):
self.display_page = self.display_connecting
watchlist = [
'ap.enabled', 'ap.mode', 'ap.heading_command', 'gps.source',
'wind.source', 'servo.controller', 'servo.flags',
'imu.compass_calibration', 'imu.compass_calibration_sigmapoints',
'imu.alignmentQ'
]
poll_list = ['ap.heading']
nalist = watchlist + poll_list + gains + \
['imu.pitch', 'imu.heel', 'ap.runtime', 'ap.version',
'imu.loopfreq', 'imu.uptime',
'imu.heading',
'imu.compass_calibration_age',
'imu.heading_lowpass_constant', 'imu.headingrate_lowpass_constant',
'imu.headingraterate_lowpass_constant',
'servo.voltage', 'servo.watts', 'servo.amp_hours'] + self.initial_gets
self.last_msg = {}
for name in nalist:
self.last_msg[name] = _('N/A')
for name in ['gps.source', 'wind.source']:
self.last_msg[name] = 'none'
self.last_msg['ap.heading_command'] = 0
self.last_msg['imu.heading_offset'] = 0
host = ''
if len(sys.argv) > 1:
host = sys.argv[1]
def on_con(client):
self.value_list = {}
request = {'method': 'list'}
client.send(request)
for name in watchlist:
client.watch(name)
try:
self.client = SignalKClient(on_con, host)
self.display_page = self.display_control
print 'connected'
for request in self.initial_gets:
self.get(request)
except:
self.client = False
time.sleep(1)
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
def background_thread(self):
try:
spi = spidev.SpiDev()
spi.open(0, 1)
spi.max_speed_hz=5000
except Exception as e:
print('failed to open spi device', e)
exit(1)
watchlist = ['ap.enabled', 'ap.heading_command']
for name in watchlist:
self.last_values[name] = 0
def on_con(client):
for name in watchlist:
client.watch(name)
socketio.emit('pypilot', 'Connected')
self.client = False
dt = 0
lastpollheading = time.time()
while True:
if self.client:
try:
while True:
msg = self.client.receive_single()
if not msg:
break
name, value = msg
self.last_values[name] = value['value']
except Exception as e:
socketio.emit('pypilot', 'Disconnected' + str(e))
self.client = False
if not self.client:
socketio.sleep(3)
try:
self.client = SignalKClient(on_con, host)
print('connected', host)
except Exception as e:
print('failed to connect', e)
t = time.time()
dtc = t - self.last_code_time
if dtc > 8 and self.last_code:
self.last_code = False
socketio.emit('rfcode', 'N/A')
socketio.emit('action', '')
# poll heading once per second if not enabled
dtp = t - lastpollheading
if self.client and dtp > 1 and not self.last_values['ap.enabled']:
self.client.get('ap.heading')
lastpollheading = t
# timeout manual move
if self.servo_timeout:
dtt = t - self.servo_timeout
if dtt > 0:
self.client.set('servo.command', 0) #stop
self.servo_timeout = 0
x = spi.xfer([0, 0, 0, 0])
if not any(x):
socketio.sleep(dt)
dt = min(.1, dt+.001)
continue
dt = 0
for i in range(4):
if not x[0] and x[1] and x[2] and x[3]: # ok
code = (x[1]*256 + x[2])*256 + x[3]
if self.last_code != code or dtc > .6:
self.apply_code(code)
break
x = x[1:] + [spi.xfer([0])[0]]
spi.close()
if __name__ == '__main__':
host = ''
if len(sys.argv) > 1:
host = sys.argv[1]
def on_con(client):
watchlist = [
'imu.accel', 'imu.compass', 'imu.compass.calibration',
'imu.compass.calibration', 'imu.compass.calibration.sigmapoints',
'imu.fusionQPose'
]
for name in watchlist:
client.watch(name)
client = SignalKClient(on_con, host, autoreconnect=True)
plot = CompassCalibrationPlot()
def display():
plot.display()
glutSwapBuffers()
last = False
def mouse(button, state, x, y):
if button == GLUT_LEFT_BUTTON and state == GLUT_DOWN:
global last
last = x, y
def motion(x, y):
global last
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)
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()
def background_thread(self):
try:
spi = spidev.SpiDev()
spi.open(0, 1)
spi.max_speed_hz = 5000
except Exception as e:
print('failed to open spi device', e)
exit(1)
watchlist = ['ap.enabled', 'ap.heading_command']
for name in watchlist:
self.last_values[name] = 0
def on_con(client):
for name in watchlist:
client.watch(name)
socketio.emit('pypilot', 'Connected')
self.client = False
dt = 0
while True:
if self.client:
try:
while True:
msg = self.client.receive_single()
if not msg:
break
name, value = msg
self.last_values[name] = value
except Exception as e:
socketio.emit('pypilot', 'Disconnected' + str(e))
self.client = False
if not self.client:
socketio.sleep(3)
try:
self.client = SignalKClient(on_con, host)
except Exception as e:
print('failed to connect', e)
x = spi.xfer([0, 0, 0, 0])
if not any(x):
socketio.sleep(dt)
dt = min(.1, dt + .001)
continue
dt = 0
for i in range(4):
if not x[0] and x[1] and x[2] and x[3]: # ok
code = (x[1] * 256 + x[2]) * 256 + x[3]
print('code', code)
for name in self.actions:
action = self.actions[name]
if code in action.codes:
action.trigger(self.client)
socketio.emit('rfcode', "%X" % code)
break
x = x[1:] + [spi.xfer([0])[0]]
spi.close()