def receive_messages(self, event):
if not self.client:
try:
host, port = self.host_port
self.client = pypilotClient(self.on_con,
host,
port,
autoreconnect=False)
self.timer.Start(50)
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]] or result[0] == 'timestamp':
if self.plot.read_data(result):
refresh = True
if refresh:
self.glArea.Refresh()
def run(self):
from signal import signal
def cleanup(a, b):
print('time spent load time', self.load_time.time())
print('time spent fit time', self.fit_time.time())
print('time spent total', self.total_time.time())
exit(0)
signal(2, cleanup)
# add predictions to the list of sensors
for p in self.conf['predictions']:
if not p in self.conf['sensors']:
self.conf['sensors'].append(p)
t0 = time.monotonic()
print('connecting to', self.host)
self.client = pypilotClient(self.host)
watches = self.conf['sensors'] + self.conf['state'] + 'ap.enabled' + 'timestamp'
for name in watches:
client.watch(name)
while True:
self.receive()
if time.monotonic() - t0 > 600:
self.save()
def receive_messages(self, event):
if not self.client:
try:
host, port = self.host_port
self.client = pypilotClient(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("pypilot 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:
t = str(type(self.controls[name]))
if t == "<class 'wx._controls.Choice'>" or t == "<class 'wx._core.Choice'>":
if not self.controls[name].SetStringSelection(
value):
print(
'warning, invalid choice value specified')
elif t == "<class 'wx._controls.Slider'>" or t == "<class 'wx._core.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 main():
def on_con(client):
print('conected to pypilot server')
client.watch('imu.heading_lowpass')
client.watch('imu.pitch')
client.watch('imu.roll')
try:
subprocess.check_output(['systemctl', 'is-enabled', 'pypilot_boatimu']).decode(sys.stdin.encoding)
pypilot_boatimu = True
except: pypilot_boatimu = False
try:
subprocess.check_output(['systemctl', 'is-enabled', 'pypilot']).decode(sys.stdin.encoding)
pypilot = True
except: pypilot = False
if pypilot_boatimu or pypilot:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
client = False
while True:
time.sleep(0.5)
try:
if not client:
client = pypilotClient(on_con, 'localhost')
except:
time.sleep(3)
continue
try:
result = client.receive()
except:
print('disconnected from pypilot server')
client = False
continue
headingValue = ''
rollValue = ''
pitchValue = ''
values = ''
for i in result:
if 'imu.heading_lowpass' in i: headingValue = result[i]['value']*0.017453293
if 'imu.roll' in i: rollValue = result[i]['value']*0.017453293
if 'imu.pitch' in i: pitchValue = result[i]['value']*0.017453293
if headingValue and rollValue and pitchValue:
if pypilot_boatimu:
values += '{"path": "navigation.headingMagnetic","value":'+str(headingValue)+'}'
values += ','
values += '{"path": "navigation.attitude","value":{"roll":'+str(rollValue)+',"pitch":'+str(pitchValue)+',"yaw":null}}'
SignalK = '{"updates":[{"$source":"OpenPlotter.I2C.pypilot","values":['+values+']}]}\n'
sock.sendto(SignalK.encode('utf-8'), ('127.0.0.1', 20220))
def __init__(self):
wx.Frame.__init__(self, None, title="pypilot client", size=(1000, 600))
host = ''
if len(sys.argv) > 1:
host = sys.argv[1]
self.client = pypilotClient(host)
self.connected = 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)
self.Refresh(None)
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)
def __init__(self):
super(CalibrationDialog, self).__init__(None)
self.host = ''
if len(sys.argv) > 1:
self.host = sys.argv[1]
self.client = False
self.accel_calibration_plot = calibration_plot.AccelCalibrationPlot()
self.accel_calibration_glContext = wx.glcanvas.GLContext(
self.AccelCalibration)
self.compass_calibration_plot = calibration_plot.CompassCalibrationPlot(
)
self.compass_calibration_glContext = wx.glcanvas.GLContext(
self.CompassCalibration)
self.boat_plot = boatplot.BoatPlot()
self.boat_plot_glContext = wx.glcanvas.GLContext(self.BoatPlot)
self.lastmouse = False
self.alignment_count = 0
self.timer = wx.Timer(self, self.ID_MESSAGES)
self.timer.Start(50)
self.Bind(wx.EVT_TIMER, self.receive_messages, id=self.ID_MESSAGES)
self.heading_offset_timer = wx.Timer(self, self.ID_HEADING_OFFSET)
self.Bind(wx.EVT_TIMER,
lambda e: self.sHeadingOffset.SetValue(
round3(self.pypilot_heading_offset)),
id=self.ID_HEADING_OFFSET)
self.have_rudder = False
self.fusionQPose = [1, 0, 0, 0]
self.alignmentQ = [1, 0, 0, 0]
self.controltimes = {}
self.client = pypilotClient(self.host)
# clear out plots
self.accel_calibration_plot.points = []
self.compass_calibration_plot.points = []
self.settings = {}
self.set_watches()
def receive_messages(self, event):
if not self.client:
try:
self.client = pypilotClient(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
except Exception as e:
print(e)
def receive(self):
if self.playback_file:
line = self.playback_file.readline()
if not line:
print('end of file')
exit(0)
msg = json.loads(line)
for name in msg:
self.receive_single(name, msg[name])
return
if not self.client:
print('connecting to', self.host)
# couldn't load try to connect
watches = self.conf['sensors'] + list(self.conf['state'])
watches.append('ap.enabled')
watches.append('timestamp')
def on_con(client):
for name in watches:
client.watch(name)
self.client = pypilotClient(on_con, self.host, autoreconnect=False)
msg = self.client.receive_single(1)
while msg:
if self.record_file:
d = {msg[0]: msg[1]['value']}
self.record_file.write(json.dumps(d) + '\n')
self.record_file.lines += 1
if self.record_file.lines % 100 == 0:
sys.stdout.write('recording ' +
str(self.record_file.lines) + '\r')
sys.stdout.flush()
else:
name, data = msg
value = data['value']
self.receive_single(name, value)
msg = self.client.receive_single(-1)
def connect(self):
for name in ['gps.source', 'wind.source']:
self.last_msg[name] = 'none'
self.last_msg['ap.enabled'] = False
self.last_msg['ap.heading_command'] = 0
self.last_msg['imu.heading_offset'] = 0
if self.config['remote']:
host = self.config['host']
else:
host = 'localhost'
def on_con(client):
self.value_list = client.list_values(10)
self.watchlist = ['ap.enabled', 'ap.heading_command'
] + self.lcd.watchlist
for name in self.watchlist:
client.watch(name)
for request in self.lcd.initial_gets:
client.get(request)
try:
self.client = pypilotClient(on_con, host)
if self.value_list:
print('connected')
self.web.set_status('connected')
else:
self.client.disconnect()
print('no value list!')
self.client = False
self.web.set_status('disconnected')
except Exception as e:
print('hat exception', e)
self.client = False
time.sleep(1)
def __init__(self):
# read config
self.config = {
'remote': False,
'host': '127.0.0.1',
'actions': {},
'pi.ir': True,
'arduino.ir': False,
'arduino.nmea.in': False,
'arduino.nmea.out': False,
'arduino.nmea.baud': 4800,
'lcd': {}
}
self.configfilename = os.getenv('HOME') + '/.pypilot/hat.conf'
print('loading config file:', self.configfilename)
try:
file = open(self.configfilename)
config = pyjson.loads(file.read())
file.close()
for name in config:
self.config[name] = config[name]
except Exception as e:
print('config failed:', e)
try:
configfile = '/proc/device-tree/hat/custom_0'
f = open(configfile)
hat_config = pyjson.loads(f.read())
f.close()
print('loaded device tree hat config')
if not 'hat' in self.config or hat_config != self.config['hat']:
self.config['hat'] = hat_config
print('writing device tree hat to hat.conf')
self.write_config()
except Exception as e:
print('failed to load', configfile, ':', e)
if not 'hat' in self.config:
if os.path.exists('/dev/spidev0.0'):
print(
'assuming original 26 pin tinypilot with nokia5110 display'
)
self.config['hat'] = {
'lcd': {
'driver': 'nokia5110',
'port': '/dev/spidev0.0'
},
'lirc': 'gpio4'
}
self.write_config()
self.servo_timeout = time.monotonic() + 1
self.last_msg = {}
self.last_msg['ap.enabled'] = False
self.last_msg['ap.heading_command'] = 0
if self.config['remote']:
host = self.config['host']
else:
host = 'localhost'
self.client = pypilotClient(host)
self.client.registered = False
self.watchlist = ['ap.enabled', 'ap.heading_command']
for name in self.watchlist:
self.client.watch(name)
self.lcd = lcd.LCD(self)
self.gpio = gpio.gpio()
self.arduino = Arduino(self)
self.poller = select.poll()
self.poller.register(self.arduino.pipe, select.POLLIN)
self.lirc = lircd.lirc(self.config)
self.lirc.registered = False
self.keytimes = {}
self.keytimeouts = {}
# keypad for lcd interface
self.actions = []
keypadnames = [
'auto', 'menu', 'port1', 'starboard1', 'select', 'port10',
'starboard10', 'tack', 'dodge_port', 'dodge_starboard'
]
for i in range(len(keypadnames)):
self.actions.append(ActionKeypad(self.lcd, i, keypadnames[i]))
# stateless actions for autopilot control
self.actions += [
ActionEngage(self),
ActionPypilot(self, 'disengage', 'ap.enabled', False),
ActionHeading(self, 1),
ActionHeading(self, -1),
ActionHeading(self, 2),
ActionHeading(self, -2),
ActionHeading(self, 5),
ActionHeading(self, -5),
ActionHeading(self, 10),
ActionHeading(self, -10),
ActionPypilot(self, 'compassmode', 'ap.mode', 'compass'),
ActionPypilot(self, 'gpsmode', 'ap.mode', 'gps'),
ActionPypilot(self, 'windmode', 'ap.mode', 'wind'),
ActionPypilot(self, 'center', 'servo.position', 0),
ActionTack(self, 'tackport', 'port'),
ActionTack(self, 'tackstarboard', 'starboard'),
ActionNone()
]
for action in self.actions:
if action.name in self.config['actions']:
action.keys = self.config['actions'][action.name]
self.web = Web(self)
def cleanup(signal_number, frame=None):
print('got signal', signal_number, 'cleaning up', os.getpid())
childpids = []
processes = [self.arduino, self.web]
for process in processes:
if process.process:
childpids.append(process.process.pid)
if signal_number == signal.SIGCHLD:
pid = os.waitpid(-1, os.WNOHANG)
if not pid[0] in childpids:
print('subprocess returned', pid, childpids)
# flask or system makes process at startup that dies
return
print('child process', pid, childpids)
while childpids:
pid = childpids.pop()
#print('kill!', pid, childpids, os.getpid())
try:
os.kill(pid, signal.SIGTERM) # get backtrace
except ProcessLookupError:
pass # ok, process is already terminated
sys.stdout.flush()
for process in processes:
process.process = False
if signal_number != 'atexit':
raise KeyboardInterrupt # to get backtrace on all processes
sys.stdout.flush()
for s in range(1, 16):
if s != 9 and s != 13:
signal.signal(s, cleanup)
signal.signal(signal.SIGCHLD, cleanup)
import atexit
atexit.register(lambda: cleanup('atexit'))
def CalibrationProcess(cal_pipe):
import os
if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()):
print(
'warning, failed to make calibration process idle, trying renice')
if os.system("renice 20 %d" % os.getpid()):
print('warning, failed to renice calibration process')
accel_cal = SigmaPoints(.05**2, 12, 10)
compass_cal = SigmaPoints(1.1**2, 24, 3)
accel_calibration = [0, 0, 0, 1]
norm = [0, 0, 1]
def on_con(client):
client.watch('imu.alignmentQ')
client.watch('imu.compass.calibration')
while True:
time.sleep(2)
try:
client = pypilotClient(on_con, 'localhost', autoreconnect=True)
break
except Exception as e:
print('nmea process failed to connect pypilot', e)
def debug(name):
def debug_by_name(*args):
s = ''
for a in args:
s += str(a) + ' '
if client:
client.set('imu.' + name + '.calibration.log', s)
return debug_by_name
while True:
t = time.time()
addedpoint = False
while time.time() - t < calibration_fit_period:
# receive pypilot messages
msg = client.receive()
for name in msg:
value = msg[name]['value']
if name == 'imu.alignmentQ' and value:
norm = quaternion.rotvecquat([0, 0, 1], value)
compass_cal.Reset()
elif name == 'imu.compass.calibration':
compass_calibration = value[0]
# receive calibration data
p = cal_pipe.recv(1)
if p:
if 'accel' in p:
accel_cal.AddPoint(p['accel'])
addedpoint = True
if 'compass' in p:
compass_cal.AddPoint(p['compass'], p['down'])
addedpoint = True
# send updated sigmapoints as well
cals = {'accel': accel_cal, 'compass': compass_cal}
for name in cals:
cal = cals[name]
if cal.Updated():
points = cal.Points()
client.set('imu.' + name + '.calibration.sigmapoints',
points)
if not addedpoint: # don't bother to run fit if no new data
continue
accel_cal.RemoveOlder(10 * 60) # 10 minutes
fit = FitAccel(debug('accel'), accel_cal)
if fit: # reset compass sigmapoints on accel cal
dist = vector.dist(fit[0][:3], accel_calibration[:3])
if dist > .01: # only update when bias changes more than this
if dist > .08: # reset compass cal from large change in accel bias
compass_cal.Reset()
accel_calibration = fit[0]
client.set('imu.accel.calibration', fit)
compass_cal.RemoveOlder(20 * 60) # 20 minutes
fit = FitCompass(debug('compass'), compass_cal, compass_calibration,
norm)
if fit:
client.set('imu.compass.calibration', fit)
compass_calibration = fit[0]
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()
client.watch('nmea.client')
while True:
time.sleep(2)
try:
self.client = pypilotClient(on_con,
'localhost',
autoreconnect=True)
break
except Exception as e:
print('nmea process failed to connect pypilot', e)
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setblocking(0)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.client_socket = False
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',
'nmea.client': ''
}
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, fd)
elif sock == server:
self.new_socket_connection(*server.accept())
elif sock == pipe:
while True: # receive all messages in pipe
msg = self.pipe.recv()
if not msg:
break
msg += '\r\n'
# relay nmea message from server to all tcp sockets
for sock in self.sockets:
sock.send(msg)
pass
elif flag & select.POLLIN:
if not sock.recv():
self.socket_lost(sock, fd)
else:
while True:
line = sock.readline()
if not line:
break
self.receive_nmea(line, 'socket' + str(sock.uid),
msgs)
else:
print('nmea bridge unhandled poll flag', flag)
t2 = time.time()
# send any parsed nmea messages the server might care about
if msgs:
if self.pipe.send(msgs):
msgs = {}
t3 = time.time()
# receive pypilot messages
try:
pypilot_msgs = self.client.receive()
for name in pypilot_msgs:
value = pypilot_msgs[name]['value']
self.last_values[name] = value
except Exception as e:
print('nmea exception receiving:', e)
t4 = time.time()
# flush sockets
for sock in self.sockets:
sock.flush()
t5 = time.time()
# reconnect client tcp socket
nmea_client = self.last_values['nmea.client']
if self.client_socket:
if self.client_socket.nmea_client != nmea_client:
self.client_socket.socket.close(
) # address has changed, close connection
elif nmea_client:
try:
self.connect_client(nmea_client)
except Exception as e:
print('failed to create nmea socket as host:port',
nmea_client, e)
self.last_values[
'nmea.client'] = '' # don't try until changed
t6 = time.time()
# run tcp nmea traffic at rate of 10hz
if t6 - t1 > .1:
print('nmea process loop too slow:', t1 - t0, t2 - t1, t3 - t2,
t4 - t3, t5 - t4, t6 - t5)
else:
dt = .1 - (t6 - t0)
if dt > 0 and dt < .1:
time.sleep(dt)
def on_connect(self):
print('Client connected', request.sid)
client = pypilotClient()
self.clients[request.sid] = client
print('ERROR!', down, cal_sphere, e)
glEnd()
self.draw_points()
if __name__ == '__main__':
host = False
if len(sys.argv) > 1:
host = sys.argv[1]
watchlist = [
'imu.accel', 'imu.compass', 'imu.compass.calibration',
'imu.compass.calibration', 'imu.compass.calibration.sigmapoints',
'imu.fusionQPose'
]
client = pypilotClient(host)
for name in watchlist:
client.watch(name)
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
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', 'imu.alignmentQ'
]
for name in watchlist:
client.watch(name)
client = pypilotClient(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
