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 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 __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
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 __init__(self):
super(SignalKScope, self).__init__(None)
self.plot = SignalKPlot()
self.glContext = wx.glcanvas.GLContext(self.glArea)
self.client = SignalKClientFromArgs(sys.argv[:2], True, self.on_con)
self.host_port = self.client.host_port
self.client.autoreconnect = False
self.value_list = self.client.list_values()
self.plot.init(self.value_list)
self.watches = {}
watches = sys.argv[1:]
for name in sorted(self.value_list):
if self.value_list[name]['type'] != 'SensorValue':
continue
i = self.clValues.Append(name)
self.watches[name] = False
for arg in watches:
if arg == name:
self.clValues.Check(i, True)
self.watches[name] = True
watches.remove(name)
for arg in watches:
print('value not found:', arg)
self.on_con(self.client)
self.timer = wx.Timer(self, wx.ID_ANY)
self.Bind(wx.EVT_TIMER, self.receive_messages, id=wx.ID_ANY)
self.timer.Start(100)
self.sTime.SetValue(self.plot.disptime)
self.plot_reshape = False
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase):
ID_MESSAGES = 1000
ID_CALIBRATE_SERVO = 1001
ID_HEADING_OFFSET = 1002
ID_REQUEST_MSG = 1003
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.dsServoMaxCurrent.SetIncrement(.1)
self.dsServoMaxCurrent.SetDigits(1)
self.dsServoMaxCurrent.Bind(wx.EVT_SPINCTRLDOUBLE, self.onMaxCurrent)
self.dsServoMaxControllerTemp.SetRange(45, 100)
self.dsServoMaxControllerTemp.Bind(wx.EVT_SPINCTRL,
self.onMaxControllerTemp)
self.dsServoMaxMotorTemp.SetRange(30, 100)
self.dsServoMaxMotorTemp.Bind(wx.EVT_SPINCTRL, self.onMaxMotorTemp)
self.dsServoCurrentFactor.SetRange(.8, 1.2)
self.dsServoCurrentFactor.SetIncrement(.0016)
self.dsServoCurrentFactor.SetDigits(4)
self.dsServoCurrentFactor.Bind(wx.EVT_SPINCTRLDOUBLE,
self.onCurrentFactor)
self.dsServoCurrentOffset.SetRange(-1.2, 1.2)
self.dsServoCurrentOffset.SetIncrement(.01)
self.dsServoCurrentOffset.SetDigits(2)
self.dsServoCurrentOffset.Bind(wx.EVT_SPINCTRLDOUBLE,
self.onCurrentOffset)
self.dsServoVoltageFactor.SetRange(.8, 1.2)
self.dsServoVoltageFactor.SetIncrement(.0016)
self.dsServoVoltageFactor.SetDigits(4)
self.dsServoVoltageFactor.Bind(wx.EVT_SPINCTRLDOUBLE,
self.onVoltageFactor)
self.dsServoVoltageOffset.SetRange(-1.2, 1.2)
self.dsServoVoltageOffset.SetIncrement(.01)
self.dsServoVoltageOffset.SetDigits(2)
self.dsServoVoltageOffset.Bind(wx.EVT_SPINCTRLDOUBLE,
self.onVoltageOffset)
self.dsServoMaxSlewSpeed.SetRange(0, 100)
self.dsServoMaxSlewSpeed.Bind(wx.EVT_SPINCTRL, self.onMaxSlewSpeed)
self.dsServoMaxSlewSlow.SetRange(0, 100)
self.dsServoMaxSlewSlow.Bind(wx.EVT_SPINCTRL, self.onMaxSlewSlow)
self.dsServoGain.SetIncrement(.1)
self.dsServoGain.SetDigits(1)
self.dsServoGain.Bind(wx.EVT_SPINCTRLDOUBLE, self.onServoGain)
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.signalk_heading_offset)),
id=self.ID_HEADING_OFFSET)
self.servo_timer = wx.Timer(self, self.ID_CALIBRATE_SERVO)
self.Bind(wx.EVT_TIMER,
self.calibrate_servo_timer,
id=self.ID_CALIBRATE_SERVO)
self.have_rudder = False
self.request_msg_timer = wx.Timer(self, self.ID_REQUEST_MSG)
self.Bind(wx.EVT_TIMER, self.request_msg, id=self.ID_REQUEST_MSG)
self.request_msg_timer.Start(250)
self.servoprocess = False
self.fusionQPose = [1, 0, 0, 0]
self.controltimes = {}
def set_watches(self, client):
if not client:
return
watchlist = [[
'imu.fusionQPose', 'imu.alignmentCounter', 'imu.heading',
'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel',
'imu.heading_offset'
],
[
'imu.accel.calibration', 'imu.accel.calibration.age',
'imu.accel', 'imu.accel.calibration.sigmapoints',
'imu.accel.calibration.locked'
],
[
'imu.fusionQPose', 'imu.compass.calibration',
'imu.compass.calibration.age', 'imu.compass',
'imu.compass.calibration.sigmapoints',
'imu.compass.calibration.locked'
],
[
'servo.flags', 'servo.max_current',
'servo.max_controller_temp', 'servo.max_motor_temp',
'servo.current.factor', 'servo.current.offset',
'servo.voltage.factor', 'servo.voltage.offset',
'servo.max_slew_speed', 'servo.max_slew_slow',
'servo.gain'
],
[
'rudder.offset', 'rudder.scale',
'rudder.nonlinearity', 'rudder.range',
'rudder.calibration_state'
]]
pageindex = 0
for pagelist in watchlist:
for name in pagelist:
client.watch(name, False)
pageindex = self.m_notebook.GetSelection()
for name in watchlist[pageindex]:
client.watch(name)
def on_con(self, client):
self.set_watches(client)
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 request_msg(self, event):
if not self.client:
return
page_gets = [[], [], [], ['servo.voltage', 'servo.current'],
['rudder.angle']]
i = self.m_notebook.GetSelection()
for name in page_gets[i]:
self.client.get(name)
def UpdateControl(self, control, update):
t = time.time()
if not control in self.controltimes or t - self.controltimes[
control] > .5:
update()
self.controltimes[control] = t
def UpdateLabel(self, label, value):
self.UpdateControl(label, lambda: label.SetLabel(str(value)))
def UpdatedSpin(self, dspin, value):
self.UpdateControl(dspin, lambda: dspin.SetValue(value))
def receive_message(self, msg):
name, data = msg
value = data['value']
if self.m_notebook.GetSelection() == 1:
self.accel_calibration_plot.read_data(msg)
if name == 'imu.accel':
self.AccelCalibration.Refresh()
elif name == 'imu.accel.calibration':
self.stAccelCal.SetLabel(str(round3(value)))
elif name == 'imu.accel.calibration.age':
self.stAccelCalAge.SetLabel(str(value))
elif name == 'imu.accel.calibration.locked':
self.cbAccelCalibrationLocked.SetValue(value)
elif self.m_notebook.GetSelection() == 2:
self.compass_calibration_plot.read_data(msg)
if name == 'imu.compass':
self.CompassCalibration.Refresh()
elif name == 'imu.compass.calibration':
self.stCompassCal.SetLabel(str(round3(value[0])))
elif name == 'imu.compass.calibration.age':
self.stCompassCalAge.SetLabel(str(value))
elif name == 'imu.compass.calibration.locked':
self.cbCompassCalibrationLocked.SetValue(value)
elif self.m_notebook.GetSelection() == 0:
if name == 'imu.alignmentQ':
self.stAlignment.SetLabel(
str(round3(value)) + ' ' +
str(math.degrees(pypilot.quaternion.angle(value))))
elif name == 'imu.fusionQPose':
if not value:
return # no imu! show warning?
if self.cCoords.GetSelection() == 1:
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q, self.fusionQPose)
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q, pypilot.quaternion.conjugate(value))
elif self.cCoords.GetSelection() == 2:
ang = pypilot.quaternion.toeuler(
self.fusionQPose)[2] - pypilot.quaternion.toeuler(
value)[2]
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q,
pypilot.quaternion.angvec2quat(ang, [0, 0, 1]))
self.fusionQPose = value
self.BoatPlot.Refresh()
elif name == 'imu.alignmentCounter':
self.gAlignment.SetValue(100 - value)
enable = value == 0
self.bLevel.Enable(enable)
elif name == 'imu.pitch':
self.stPitch.SetLabel(str(round3(value)))
elif name == 'imu.roll':
self.stRoll.SetLabel(str(round3(value)))
elif name == 'imu.heel':
self.stHeel.SetLabel(str(round3(value)))
elif name == 'imu.heading':
self.stHeading.SetLabel(str(round3(value)))
elif name == 'imu.heading_offset':
self.signalk_heading_offset = value
self.heading_offset_timer.Start(1000, True)
elif self.m_notebook.GetSelection() == 3:
if name == 'servo.flags':
self.UpdateLabel(self.stServoFlags, value)
elif name == 'servo.calibration':
s = ''
for name in value:
s += name + ' = ' + str(value[name]) + '\n'
self.stServoCalibration.SetLabel(s)
self.SetSize(wx.Size(self.GetSize().x + 1, self.GetSize().y))
elif name == 'servo.max_current':
self.UpdatedSpin(self.dsServoMaxCurrent, round3(value))
elif name == 'servo.max_controller_temp':
self.UpdatedSpin(self.dsServoMaxControllerTemp, value)
elif name == 'servo.max_motor_temp':
self.UpdatedSpin(self.dsServoMaxMotorTemp, value)
elif name == 'servo.current.factor':
self.UpdatedSpin(self.dsServoCurrentFactor, round(value, 4))
elif name == 'servo.current.offset':
self.UpdatedSpin(self.dsServoCurrentOffset, value)
elif name == 'servo.voltage.factor':
self.UpdatedSpin(self.dsServoVoltageFactor, round(value, 4))
elif name == 'servo.voltage.offset':
self.UpdatedSpin(self.dsServoVoltageOffset, value)
elif name == 'servo.max_slew_speed':
self.UpdatedSpin(self.dsServoMaxSlewSpeed, value)
elif name == 'servo.max_slew_slow':
self.UpdatedSpin(self.dsServoMaxSlewSlow, value)
elif name == 'servo.gain':
self.UpdatedSpin(self.dsServoGain, value)
elif name == 'servo.voltage':
self.UpdateLabel(self.m_stServoVoltage, (str(round3(value))))
elif name == 'servo.current':
self.UpdateLabel(self.m_stServoCurrent, (str(round3(value))))
elif self.m_notebook.GetSelection() == 4:
if name == 'rudder.angle':
self.UpdateLabel(self.stRudderAngle, str(round3(value)))
self.have_rudder = type(value) != type(bool)
elif name == 'rudder.offset':
self.UpdateLabel(self.stRudderOffset, str(round3(value)))
elif name == 'rudder.scale':
self.UpdateLabel(self.stRudderScale, (str(round3(value))))
elif name == 'rudder.nonlinearity':
self.UpdateLabel(self.stRudderNonlinearity, str(round3(value)))
elif name == 'rudder.range':
self.UpdatedSpin(self.sRudderRange, value)
def servo_console(self, text):
self.stServoCalibrationConsole.SetLabel(
self.stServoCalibrationConsole.GetLabel() + text + '\n')
def calibrate_servo_timer(self, event):
if not self.servoprocess:
return
self.servoprocess.poll()
print('servotimer', self.servoprocess.returncode,
self.servoprocess.returncode == None)
if self.servoprocess.returncode == None:
self.servoprocess.communicate()
line = self.servoprocess.stdout.readline()
print('line', line)
if line:
self.servo_console(line)
self.servo_timer.Start(150, True)
else:
self.bCalibrateServo.Enable()
if self.servoprocess.returncode == 0:
file = open('servo_calibration')
calibration = json.loads(file.readline())
self.client.set('servo.calibration', calibration)
self.servo_console('calibration sent.')
else:
self.servo_console('calibration failed.')
self.servoprocess = False
def PageChanged(self, event):
self.set_watches(self.client)
def onKeyPressAccel(self, event):
self.onKeyPress(event, self.compass_calibration_plot)
def onKeyPressCompass(self, event):
self.onKeyPress(event, self.compass_calibration_plot)
def onKeyPress(self, event, plot):
signalk.scope_wx.wxglutkeypress(event, plot.special, plot.key)
def onClearAccel(self, event):
self.accel_calibration_plot.points = []
def onClearCompass(self, event):
self.compass_calibration_plot.points = []
def onAccelCalibrationLocked(self, event):
self.client.set('imu.accel.calibration.locked',
self.cbAccelCalibrationLocked.GetValue())
def onCompassCalibrationLocked(self, event):
self.client.set('imu.compass.calibration.locked',
self.cbCompassCalibrationLocked.GetValue())
def onCalibrationLocked(self, sensor, ctrl):
self.client.set('imu.' + sensor + '.calibration.locked',
self.ctrl.GetValue())
def onMouseEventsAccel(self, event):
self.AccelCalibration.SetFocus()
self.onMouseEvents(event, self.AccelCalibration,
self.accel_calibration_plot)
def onMouseEventsCompass(self, event):
self.CompassCalibration.SetFocus()
self.onMouseEvents(event, self.CompassCalibration,
self.compass_calibration_plot)
def onMouseEvents(self, event, canvas, plot):
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \
pos[1] - self.lastmouse[1])
canvas.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
canvas.Refresh()
while rotation > 0:
plot.userscale /= .9
rotation -= 1
while rotation < 0:
plot.userscale *= .9
rotation += 1
def onPaintGLAccel(self, event):
self.onPaintGL(self.AccelCalibration, self.accel_calibration_plot,
self.accel_calibration_glContext)
def onPaintGLCompass(self, event):
self.onPaintGL(self.CompassCalibration, self.compass_calibration_plot,
self.compass_calibration_glContext)
def onPaintGL(self, canvas, plot, context):
wx.PaintDC(canvas)
canvas.SetCurrent(context)
plot.display()
canvas.SwapBuffers()
def onSizeGLAccel(self, event):
self.accel_calibration_plot.reshape(event.GetSize().x,
event.GetSize().y)
def onSizeGLCompass(self, event):
self.compass_calibration_plot.reshape(event.GetSize().x,
event.GetSize().y)
def StartAlignment(self):
self.client.set('imu.alignmentCounter', 100)
def onResetAlignment(self, event):
self.client.set('imu.alignmentQ', [1, 0, 0, 0])
def onLevel(self, event):
self.StartAlignment()
def onIMUHeadingOffset(self, event):
self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue())
self.heading_offset_timer.Stop()
def onKeyPressBoatPlot(self, event):
self.BoatPlot.SetFocus()
k = '%c' % (event.GetKeyCode() & 255)
if not event.GetModifiers() & wx.MOD_SHIFT:
k = k.lower()
self.BoatPlot.Refresh()
def onMouseEventsBoatPlot(self, event):
self.BoatPlot.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1]
q = pypilot.quaternion.angvec2quat(
(dx**2 + dy**2)**.4 / 180 * math.pi, [dy, dx, 0])
self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q)
self.BoatPlot.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
self.BoatPlot.Refresh()
while rotation > 0:
self.boat_plot.Scale /= .9
rotation -= 1
while rotation < 0:
self.boat_plot.Scale *= .9
rotation += 1
def onPaintGLBoatPlot(self, event):
wx.PaintDC(self.BoatPlot)
self.BoatPlot.SetCurrent(self.boat_plot_glContext)
# stupid hack
self.boat_plot.reshape(self.BoatPlot.GetSize().x,
self.BoatPlot.GetSize().y)
self.boat_plot.display(self.fusionQPose)
self.BoatPlot.SwapBuffers()
def onSizeGLBoatPlot(self, event):
self.boat_plot.reshape(event.GetSize().x, event.GetSize().y)
self.BoatPlot.Refresh()
def onTextureCompass(self, event):
self.boat_plot.texture_compass = event.IsChecked()
self.BoatPlot.Refresh()
def onIMUScope(self, event):
host, port = self.client.host_port
args = [
'python',
os.path.abspath(os.path.dirname(__file__)) +
'/../signalk/scope_wx.py', host + ':' + str(port), 'imu.pitch',
'imu.roll', 'imu.heel', 'imu.heading'
]
subprocess.Popen(args)
def onCalibrateServo(self, event):
try:
self.servoprocess = subprocess.Popen(
['python', 'servo_calibration.py', sys.argv[1]],
stdout=subprocess.PIPE)
self.servo_console('executed servo_calibration.py...')
self.servo_timer.Start(150, True)
self.bCalibrateServo.Disable()
except OSError:
self.servo_console('Failed to execute servo_calibration.py.\n')
def onMaxCurrent(self, event):
self.client.set('servo.max_current', event.GetValue())
def onMaxControllerTemp(self, event):
self.client.set('servo.max_controller_temp', event.GetValue())
def onMaxMotorTemp(self, event):
self.client.set('servo.max_motor_temp', event.GetValue())
def onCurrentFactor(self, event):
self.client.set('servo.current.factor', event.GetValue())
def onCurrentOffset(self, event):
self.client.set('servo.current.offset', event.GetValue())
def onVoltageFactor(self, event):
self.client.set('servo.voltage.factor', event.GetValue())
def onVoltageOffset(self, event):
self.client.set('servo.voltage.offset', event.GetValue())
def onMaxSlewSpeed(self, event):
self.client.set('servo.max_slew_speed', event.GetValue())
def onMaxSlewSlow(self, event):
self.client.set('servo.max_slew_slow', event.GetValue())
def onServoGain(self, event):
self.client.set('servo.gain', event.GetValue())
def onServoAutoGain(self, event):
if self.have_rudder:
self.client.set('rudder.calibration_state', 'auto gain')
else:
wx.MessageDialog(
self, _('Auto gain calibration requires a rudder sensor'),
_('Warning'), wx.OK).ShowModal()
def onRudderResetCalibration(self, event):
self.client.set('rudder.calibration_state', 'reset')
def onRudderCentered(self, event):
self.client.set('rudder.calibration_state', 'centered')
def onRudderStarboardRange(self, event):
self.client.set('rudder.calibration_state', 'starboard range')
def onRudderPortRange(self, event):
self.client.set('rudder.calibration_state', 'port range')
def onRudderRange(self, event):
self.client.set('rudder.range', event.GetValue())
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 ''
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 = ['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)
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()
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()
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()
class SignalKScope(SignalKScopeBase):
def __init__(self):
super(SignalKScope, self).__init__(None)
self.plot = SignalKPlot()
self.glContext = wx.glcanvas.GLContext(self.glArea)
self.client = SignalKClientFromArgs(sys.argv[:2], True, self.on_con)
self.host_port = self.client.host_port
self.client.autoreconnect = False
self.value_list = self.client.list_values()
self.plot.init(self.value_list)
self.watches = {}
watches = sys.argv[1:]
for name in sorted(self.value_list):
if self.value_list[name]['type'] != 'SensorValue':
continue
i = self.clValues.Append(name)
self.watches[name] = False
for arg in watches:
if arg == name:
self.clValues.Check(i, True)
self.watches[name] = True
watches.remove(name)
for arg in watches:
print('value not found:', arg)
self.on_con(self.client)
self.timer = wx.Timer(self, wx.ID_ANY)
self.Bind(wx.EVT_TIMER, self.receive_messages, id=wx.ID_ANY)
self.timer.Start(100)
self.sTime.SetValue(self.plot.disptime)
self.plot_reshape = False
def on_con(self, client):
self.plot.add_blank()
for i in range(self.clValues.GetCount()):
if self.clValues.IsChecked(i):
client.watch(self.clValues.GetString(i))
self.watches[self.clValues.GetString(i)] = True
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 onValueSelected( self, event ):
self.plot.select(self.clValues.GetStringSelection())
def onValueToggled( self, event ):
value = self.clValues.IsChecked(event.GetInt())
self.watches[event.GetString()] = value
self.client.watch(event.GetString(), value)
self.plot.add_blank(event.GetString())
def onPaintGL( self, event ):
dc = wx.PaintDC( self.glArea )
self.glArea.SetCurrent(self.glContext)
self.plot.fft_on = self.cbfftw.GetValue()
if self.plot_reshape:
self.plot.reshape(*self.plot_reshape)
self.plot_reshape = False
self.plot.display()
self.glArea.SwapBuffers()
def onSizeGL( self, event ):
self.plot_reshape = (event.GetSize().x, event.GetSize().y)
def onMouseEvents( self, event ):
self.glArea.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.RightDown():
self.plot.curtrace.center()
self.glArea.Refresh()
if event.Dragging():
offset = pos[1] - self.lastmouse[1]
self.plot.adjustoffset(offset, self.glArea.GetSize().y)
self.lastmouse = pos
self.glArea.Refresh()
rotation = event.GetWheelRotation() / 60
if rotation:
if rotation > 0:
self.plot.increasescale()
else:
self.plot.decreasescale()
self.glArea.Refresh()
def onKeyPress( self, event ):
wxglutkeypress(event, self.plot.special, self.plot.key)
self.cbfftw.SetValue(self.plot.fft_on)
self.glArea.Refresh()
def onZero( self, event ):
if self.plot.curtrace:
self.plot.curtrace.offset = 0
self.glArea.Refresh()
def onCenter( self, event ):
if self.plot.curtrace:
self.plot.curtrace.center()
self.glArea.Refresh()
def onScalePlus( self, event ):
self.plot.increasescale()
self.glArea.Refresh()
def onScaleMinus( self, event ):
self.plot.decreasescale()
self.glArea.Refresh()
def onOffsetPlus( self, event ):
self.plot.curtrace.offset -= self.plot.scale/10.0
self.glArea.Refresh()
def onOffsetMinus( self, event ):
self.plot.curtrace.offset += self.plot.scale/10.0
self.glArea.Refresh()
def onFreeze( self, event ):
self.plot.freeze = event.IsChecked()
self.glArea.Refresh()
def onReset( self, event ):
self.plot.reset()
self.glArea.Refresh()
def onTime(self, event):
self.plot.disptime = self.sTime.GetValue()
self.glArea.Refresh()
def onClose( self, event ):
self.Close()
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()
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 LCDClient():
def __init__(self, screen):
# w, h, self.bw = 44, 84, 1
# w, h, self.bw = 64, 128, 1
# w, h, self.bw = 320, 480, 0
self.config = {}
self.configfilename = os.getenv('HOME') + '/.pypilot/lcdclient.conf'
self.config['contrast'] = 60
self.config['invert'] = False
self.config['flip'] = False
self.config['language'] = 'en'
self.config['bigstep'] = 10
self.config['smallstep'] = 1
print 'loading load config file:', self.configfilename
try:
file = open(self.configfilename)
config = json.loads(file.readline())
for name in config:
self.config[name] = config[name]
except:
print 'failed to load config file:', self.configfilename
if screen:
w, h = screen.width, screen.height
mul = int(math.ceil(w / 48.0))
self.bw = 1 if w < 256 else False
width = min(w, 48*mul)
self.surface = ugfx.surface(width, width*h/w, screen.bypp, None)
self.frameperiod = .25 # 4 frames a second possible
else:
self.surface = None
self.frameperiod = 1
self.set_language(self.config['language'])
self.range_edit = False
self.modes = {'compass': self.have_compass,
'gps': self.have_gps,
'wind': self.have_wind,
'true wind': self.have_true_wind};
self.modes_list = ['compass', 'gps', 'wind', 'true wind'] # in order
self.initial_gets = gains + ['servo.min_speed', 'servo.max_speed', 'servo.max_current', 'servo.period', 'imu.alignmentCounter']
self.have_select = False
self.create_mainmenu()
self.longsleep = 30
self.display_page = self.display_connecting
self.connecting_dots = 0
self.client = False
self.keystate = {}
self.keypad = [False, False, False, False, False, False, False, False]
self.keypadup = list(self.keypad)
self.blink = black, white
self.control = False
self.wifi = False
self.overcurrent_time = 0
if orangepi:
self.pins = [11, 16, 13, 15, 12]
else:
self.pins = [17, 23, 27, 22, 18]
if GPIO:
if orangepi:
for pin in self.pins:
cmd = 'gpio -1 mode ' + str(pin) + ' up'
os.system(cmd)
GPIO.setmode(GPIO.BOARD)
else:
GPIO.setmode(GPIO.BCM)
for pin in self.pins:
try:
GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
pass
except RuntimeError:
os.system("sudo chown tc /dev/gpiomem")
GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def cbr(channel):
self.longsleep = 0
GPIO.add_event_detect(pin, GPIO.BOTH, callback=cbr, bouncetime=20)
global LIRC
if LIRC:
try:
LIRC.init('pypilot')
self.lirctime = False
except:
print 'failed to initialize lirc. is .lircrc missing?'
LIRC = None
def get(self, name):
if self.client:
self.client.get(name)
def set_language(self, name):
language = gettext.translation('pypilot_lcdclient',
os.path.abspath(os.path.dirname(__file__)) + '/locale',
languages=[name], fallback=True)
global _
_ = language.ugettext
self.config['language'] = name
def save_config(self):
try:
file = open(self.configfilename, 'w')
file.write(json.dumps(self.config) + '\n')
except IOError:
print 'failed to save config file:', self.configfilename
def create_mainmenu(self):
def value_edit(name, desc, signalk_name, value=False):
min = self.value_list[signalk_name]['min']
max = self.value_list[signalk_name]['max']
step = (max-min)/100.0
def thunk():
self.range_edit=RangeEdit(name, desc, signalk_name,
True, self, min, max, step)
return self.range_edit.display
if value:
return name, thunk, lambda : (self.last_msg[signalk_name]-min) / (max - min), signalk_name
return name, thunk
def config_edit(name, desc, config_name, min, max, step):
def thunk():
self.range_edit=RangeEdit(name, desc, config_name,
False, self, min, max, step)
return self.range_edit.display
return name, thunk
def gain():
self.menu = LCDMenu(self, _('Gain'),
map(lambda gain : value_edit(gain[gain.find('.')+1:],
gain, gain, True), gains), self.menu)
return self.display_menu
def level():
self.client.set('imu.alignmentCounter', 100)
return self.display_page
def calibrate():
def getheading():
self.get('imu.heading')
try:
return '%.1f' % self.last_msg['imu.heading']
except:
return str(self.last_msg['imu.heading'])
self.menu = LCDMenu(self, _('Calibrate'),
[(_('level'), level),
value_edit(_('heading'), getheading, 'imu.heading_offset'),
(_('info'), lambda : self.display_calibrate_info)],
self.menu)
self.menu.display_hook = self.display_calibrate
return self.display_menu
def settings():
def mode():
def set_mode(name):
def thunk():
self.client.set('ap.mode', name)
self.menu = self.menu.adam()
return self.display_control
return thunk
modes = []
index = 0
selection = 0
for mode in self.modes:
if self.modes[mode]():
modes.append((mode, set_mode(mode)))
if mode == self.last_msg['ap.mode']:
selection = index
index+=1
self.menu = LCDMenu(self, _('Mode'), modes, self.menu)
self.menu.selection = selection
return self.display_menu
def motor():
self.menu = LCDMenu(self, _('Motor'),
[value_edit(_('min speed'), _('relative'), 'servo.min_speed'),
value_edit(_('max speed'), _('relative'), 'servo.max_speed'),
value_edit(_('max current'), _('amps'), 'servo.max_current'),
value_edit(_('period'), _('seconds'), 'servo.period')],
self.menu)
return self.display_menu
def filter():
self.menu = LCDMenu(self, _('Filter'),
[value_edit(_('heading'), _('relative'), 'imu.heading_lowpass_constant'),
value_edit(_("heading'"), _('relative'), 'imu.headingraterate_lowpass_constant'),
value_edit(_("heading''"), _('relative'), 'imu.headingraterate_lowpass_constant')],
self.menu)
return self.display_menu
def control():
self.menu = LCDMenu(self, _('Control'),
[config_edit(_('small step'), _('degrees'), 'smallstep', 1, 5, 1),
config_edit(_('big step'), _('degrees'), 'bigstep', 5, 20, 5)],
self.menu)
return self.display_menu
def contrast():
self.range_edit = self.contrast_edit
return self.range_edit.display
def invert():
self.config['invert'] = not self.config['invert']
self.save_config()
return self.display_menu
def flip():
self.config['flip'] = not self.config['flip']
self.save_config()
return self.display_menu
def display():
self.menu = LCDMenu(self, _('Display'),
[config_edit(_('contrast'), '', 'contrast', 30, 90, 1),
(_('invert'), invert),
(_('flip'), flip)],
self.menu)
return self.display_menu
def language():
def set_language(name):
def thunk():
self.set_language(name)
self.save_config()
self.create_mainmenu()
return language()
return thunk
languages = [(_('English'), 'en'), (_('French'), 'fr'),
(_('Spanish'), 'es')]
index, selection = 0, 0
for lang in languages:
if lang[1] == self.config['language']:
selection = index
index+=1
self.menu = LCDMenu(self, _('Language'), map(lambda lang : (lang[0], set_language(lang[1])), languages), self.menu)
self.menu.selection = selection
return self.display_menu
self.menu = LCDMenu(self, _('Settings'),
[(_('mode'), mode),
(_('motor'), motor),
# (_('filter'), filter),
(_('control'), control),
(_('display'), display),
(_('language'), language)],
self.menu)
return self.display_menu
self.menu = LCDMenu(self, _('Menu'),
[(_('gain'), gain),
(_('calibrate'), calibrate),
(_('settings'), settings),
(_('info'), lambda : self.display_info)])
self.info_page = 0
return self.display_menu
def text(self, pos, text, size, crop=False):
pos = int(pos[0]*self.surface.width), int(pos[1]*self.surface.height)
size = int(size*self.surface.width/48)
size = font.draw(self.surface, pos, text, size, self.bw, crop)
return float(size[0])/self.surface.width, float(size[1])/self.surface.height
def fittext(self, rect, text, wordwrap=False, fill='none'):
#print 'fittext', text, wordwrap, fill
if fill != 'none':
self.surface.box(*(self.convrect(rect) + [fill]))
metric_size = 16
if wordwrap:
words = text.split(' ')
spacewidth = font.draw(self.surface, False, ' ', metric_size, self.bw)[0]
if len(words) < 2: # need at least 2 words to wrap
return self.fittext(rect, text, False, fill)
metrics = map(lambda word : (word, font.draw(self.surface, False, word, metric_size, self.bw)), words)
widths = map(lambda metric : metric[1][0], metrics)
maxwordwidth = apply(max, widths)
totalwidth = sum(widths) + spacewidth * (len(words) - 1)
size = 0
# not very efficient... just tries each x position
# for wrapping to maximize final font size
for wrappos in range(maxwordwidth, totalwidth+1):
posx, posy = 0, 0
curtext = ''
lineheight = 0
maxw = 0
for metric in metrics:
word, (width, height) = metric
if posx > 0:
width += spacewidth
if posx + width > wrappos:
curtext += '\n'
posx = 0
posy += lineheight
lineheight = 0
if posx > 0:
curtext += ' '
curtext += word
lineheight = max(lineheight, height)
posx += width
maxw = max(maxw, posx)
maxh = posy + lineheight
s = maxw, maxh
if s[0] == 0 or s[1] == 0:
continue
sw = self.surface.width * float(rect.width) / s[0]
sh = self.surface.height * float(rect.height) / s[1]
cursize = int(min(sw*metric_size, sh*metric_size))
if cursize > size:
size = cursize
text = curtext
else:
s = font.draw(self.surface, False, text, metric_size, self.bw)
if s[0] == 0 or s[1] == 0:
return 0, 0
sw = self.surface.width * float(rect.width) / s[0]
sh = self.surface.height * float(rect.height) / s[1]
size = int(min(sw*metric_size, sh*metric_size))
pos = int(rect.x*self.surface.width), int(rect.y*self.surface.height)
size = font.draw(self.surface, pos, text, size, self.bw)
return float(size[0])/self.surface.width, float(size[1])/self.surface.height
def line(self, x1, y1, x2, y2):
self.surface.line(x1, y1, x2, y2, black)
def convbox(self, x1, y1, x2, y2):
w, h = self.surface.width - 1, self.surface.height - 1
return [int(x1*w), int(y1*h), int(x2*w), int(y2*h)]
def invertrectangle(self, rect):
apply(self.surface.invert, self.convbox(rect.x, rect.y, rect.x+rect.width, rect.y+rect.height))
def convrect(self, rect):
return self.convbox(rect.x, rect.y, rect.x+rect.width, rect.y+rect.height)
def rectangle(self, rect, width = False):
if not width:
apply(self.surface.box, self.convrect(rect) + [white])
else:
box = self.convrect(rect)
apply(self.surface.invert, box)
if width:
w, h = self.surface.width - 1, self.surface.height - 1
px_width = int(max(1, min(w*width, h*width)))
self.surface.invert(box[0]+px_width, box[1]+px_width, box[2]-px_width, box[3]-px_width)
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']
poll_list = ['ap.heading']
nalist = watchlist + poll_list + gains + \
['imu.pitch', 'imu.heel', 'ap.runtime', 'ap.version',
'imu.heading',
'imu.compass_calibration_age',
'imu.heading_lowpass_constant', 'imu.headingrate_lowpass_constant',
'imu.headingraterate_lowpass_constant',
'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 round_last_msg(self, name, places):
n = 10**places
try:
return str(round(self.last_msg[name]*n)/n)
except:
return str(self.last_msg[name])
def have_compass(self):
return True
def have_gps(self):
return self.last_msg['gps.source'] != 'none'
def have_wind(self):
return self.last_msg['wind.source'] != 'none'
def have_true_wind(self):
return self.have_gps() and self.have_wind()
def display_wifi(self):
wifi = False
try:
wlan0 = open('/sys/class/net/wlan0/operstate')
line = wlan0.readline().rstrip()
wlan0.close()
if line == 'up':
wifi = True
except:
pass
if self.wifi != wifi:
wifirect = rectangle(.3, .9, .6, .12)
if wifi:
self.fittext(wifirect, 'WIFI')
else:
self.surface.box(*(self.convrect(wifirect) + [black]))
self.wifi = wifi
def display_control(self):
if not self.control:
self.surface.fill(black)
self.control = {'heading': ' ', 'heading_command': ' ', 'mode': False}
def draw_big_number(pos, num, lastnum):
def nr(x):
try:
s = str(int(round(x)))
while len(s) < 3:
s = ' ' + s
return s
except:
return x
num = nr(num)
if lastnum:
lastnum = nr(lastnum)
if num == 'N/A' and lastnum != num:
r = rectangle(pos[0], pos[1], 1, .4)
self.fittext(r, num, False, black)
return
if self.surface.width < 256:
size = 34
else:
size = 30
for i in range(3):
try:
if num[i] == lastnum[i]:
continue
except:
pass
x = pos[0]+float(i)/3
self.surface.box(*(self.convrect(rectangle(x, pos[1], .34, .4)) + [black]))
self.text((x, pos[1]), num[i], size, True)
if type(self.last_msg['ap.heading']) == type(False):
r = rectangle(0, 0, 1, .8)
self.fittext(r, _('ERROR\ncompass or gyro failure!'), True, black)
self.control['heading_command'] = 'no imu'
else:
draw_big_number((0,0), self.last_msg['ap.heading'], self.control['heading'])
self.control['heading'] = self.last_msg['ap.heading']
mode = self.last_msg['ap.mode']
if 'OVERCURRENT' in self.last_msg['servo.flags']:
self.overcurrent_time = time.time()
if self.last_msg['servo.controller'] == 'none':
if self.control['heading_command'] != 'no controller':
self.fittext(rectangle(0, .4, 1, .35), _('WARNING no motor controller'), True, black)
self.control['heading_command'] = 'no controller'
elif time.time() - self.overcurrent_time < 5: # 5 seconds
if self.control['heading_command'] != 'overcurrent':
self.fittext(rectangle(0, .4, 1, .35), _('OVER CURRENT'), True, black)
self.control['heading_command'] = 'overcurrent'
elif 'OVERTEMP' in self.last_msg['servo.flags']:
if self.control['heading_command'] != 'overtemp':
self.fittext(rectangle(0, .4, 1, .35), _('OVER TEMP'), True, black)
self.control['heading_command'] = 'overtemp'
elif mode == 'gps' and not self.have_gps():
if self.control['heading_command'] != 'no gps':
self.fittext(rectangle(0, .4, 1, .4), _('GPS not detected'), True, black)
self.control['heading_command'] = 'no gps'
elif (mode == 'wind' or mode == 'true wind') and not self.have_wind():
if self.control['heading_command'] != 'no wind':
self.fittext(rectangle(0, .4, 1, .4), _('WIND not detected'), True, black)
self.control['heading_command'] = 'no wind'
else:
# no warning, display the desired course or 'standby'
if self.last_msg['ap.enabled'] != True:
if self.control['heading_command'] != 'standby':
r = rectangle(0, .4, 1, .34)
self.fittext(r, _('standby'), False, black)
self.control['heading_command'] = 'standby'
else:
if self.control['heading_command'] != self.last_msg['ap.heading_command']:
draw_big_number((0,.4), self.last_msg['ap.heading_command'], self.control['heading_command'])
self.control['heading_command'] = self.last_msg['ap.heading_command']
self.control['mode'] = False # refresh mode
def modes():
return [self.have_compass(), self.have_gps(), self.have_wind(), self.have_true_wind()]
warning = False
if mode == 'compass':
warning = False
cal = self.last_msg['imu.compass_calibration']
if cal == 'N/A':
ndeviation = 0
else:
ndeviation = cal[1][0]
def warncal(s):
r = rectangle(0, .75, 1, .15)
self.fittext(r, s, True, white)
self.invertrectangle(r)
self.control['mode'] = 'warning'
if ndeviation == 0 and False:
warncal(_('No Cal'))
warning = True
if ndeviation > 6:
warncal(_('Bad Cal'))
warning = True
if not warning and \
(self.control['mode'] != mode or self.control['modes'] != modes()):
self.control['mode'] = mode
self.control['modes'] = modes()
#print 'mode', self.last_msg['ap.mode']
modes = {'compass': ('C', self.have_compass, rectangle(0, .74, .25, .16)),
'gps': ('G', self.have_gps, rectangle(.25, .74, .25, .16)),
'wind': ('W', self.have_wind, rectangle(.5, .74, .25, .16)),
'true wind': ('T', self.have_true_wind, rectangle(.75, .74, .25, .16))}
self.surface.box(*(self.convrect(rectangle(0, .74, 1, .18)) + [black]))
for mode in modes:
if modes[mode][1]():
self.fittext(modes[mode][2], modes[mode][0])
if self.last_msg['ap.mode'] == mode:
r = modes[mode][2]
marg = .02
self.rectangle(rectangle(r.x-marg, r.y+marg, r.width-marg, r.height), .015)
#self.control['mode'] = False # refresh mode
self.display_wifi()
def display_menu(self):
self.surface.fill(black)
self.menu.display()
def display_calibrate(self):
counter = self.last_msg['imu.alignmentCounter']
if counter:
r = rectangle(0, 0, 1, .25)
r.height = .2
self.fittext(r, ' %d%%' % (100-counter), False, black)
r.width = 1-float(counter)/100
r.height = .25
self.invertrectangle(r)
self.get('imu.alignmentCounter')
self.fittext(rectangle(0, .86, .5, .14), self.round_last_msg('imu.pitch', 1))
self.fittext(rectangle(.5, .86, .5, .14), self.round_last_msg('imu.heel', 1))
self.get('imu.pitch')
self.get('imu.heel')
def display_connecting(self):
self.surface.fill(black)
self.fittext(rectangle(0, 0, 1, .4), _('connect to server'), True)
dots = ''
for i in range(self.connecting_dots):
dots += '.'
size = self.text((0, .4), dots, 12)
self.connecting_dots += 1
if size[0] >= 1:
self.connecting_dots = 0
self.display_wifi()
def display_info(self):
self.surface.fill(black)
self.fittext(rectangle(0, 0, 1, .2), _('Info'))
if self.info_page > 1:
self.info_page = 0
y = .2
if self.info_page == 0:
spacing = .11
v = self.round_last_msg('servo.watts', 3)
runtime = self.last_msg['ap.runtime'][:7]
ah = self.round_last_msg('servo.amp_hours', 3)
items = [_('Watts'), v, _('Amp Hours'), ah, _('runtime'), runtime]
self.get('servo.watts')
self.get('servo.amp_hours')
self.get('ap.runtime')
else:
spacing = .18
ver = self.last_msg['ap.version']
items = [_('version'), ver, _('author'), 'Sean D\'Epagnier']
self.get('ap.version')
even, odd = 0, .05
for item in items:
self.fittext(rectangle(0, y, 1, spacing+even), item, True)
y += spacing + even
even, odd = odd, even
def display_calibrate_info(self):
self.surface.fill(black)
self.fittext(rectangle(0, 0, 1, .3), _('Calibrate Info'), True)
deviation = _('N/A')
try:
cal = self.last_msg['imu.compass_calibration']
ndeviation = cal[0][1]
#print ndeviation
names = [(0, _('incomplete')), (.01, _('excellent')), (.02, _('good')),
(.04, _('fair')), (.06, _('poor')), (1000, _('bad'))]
for n in names:
if ndeviation <= n[0]:
deviation = n[1]
break
except:
pass
self.fittext(rectangle(0, .3, 1, .15), _('compass'))
self.fittext(rectangle(0, .42, 1, .23), deviation)
self.fittext(rectangle(0, .65, .4, .15), _('age'))
self.fittext(rectangle(0, .8, 1, .2), self.last_msg['imu.compass_calibration_age'][:7])
#self.get('imu.compass_calibration')
self.get('imu.compass_calibration_age')
def display(self):
self.display_page()
if self.display_page != self.display_control:
self.control = False
self.wifi = False
# status cursor
w, h = self.surface.width, self.surface.height
self.blink = self.blink[1], self.blink[0]
size = h / 40
self.surface.box(w-size-1, h-size-1, w-1, h-1, self.blink[0])
def set(self, name, value):
if self.client:
self.client.set(name, value)
def menu_back(self):
if self.menu.prev:
self.menu = self.menu.prev
return self.display_menu
return self.display_control
def process_keys(self):
if self.keypadup[AUTO]: # AUTO
if self.last_msg['ap.enabled'] == False and self.display_page == self.display_control:
self.set('ap.heading_command', self.last_msg['ap.heading'])
self.set('ap.enabled', True)
else:
self.set('servo.command', 0) #stop
self.set('ap.enabled', False)
self.display_page = self.display_control
if self.keypadup[SELECT]:
if self.display_page == self.display_control:
for t in range(len(self.modes_list)):
next_mode = self.modes_list[0]
self.modes_list = self.modes_list[1:] + [next_mode]
if next_mode != self.last_msg['ap.mode'] and \
self.modes[next_mode]():
self.client.set('ap.mode', next_mode)
break
else:
self.menu = self.menu.adam() # reset to main menu
self.display_page = self.display_control
# for up and down keys providing acceration
sign = -1 if self.keypad[DOWN] or self.keypadup[DOWN] or self.keypad[LEFT] or self.keypadup[LEFT] else 1
updownup = self.keypadup[UP] or self.keypadup[DOWN]
updownheld = self.keypad[UP] > 10 or self.keypad[DOWN] > 10
speed = float(1 if updownup else min(10, .004*max(self.keypad[UP], self.keypad[DOWN])**2.5))
updown = updownheld or updownup
if self.keypadup[LEFT] or self.keypadup[RIGHT]:
updown = True
speed = 10
if self.display_page == self.display_control:
if self.keypadup[MENU] and self.surface: # MENU
self.display_page = self.display_menu
elif updown: # LEFT/RIGHT
if self.last_msg['ap.enabled']:
if self.keypadup[LEFT] or self.keypadup[RIGHT]:
speed = self.config['bigstep']
else:
speed = self.config['smallstep']
cmd = self.last_msg['ap.heading_command'] + sign*speed
self.set('ap.heading_command', cmd)
else:
self.set('servo.command', sign*(speed+8.0)/20)
elif self.display_page == self.display_menu:
if self.keypadup[UP]:
self.menu.selection -= 1
if self.menu.selection < 0:
self.menu.selection = len(self.menu.items)-1
elif self.keypadup[DOWN]:
self.menu.selection += 1
if self.menu.selection == len(self.menu.items):
self.menu.selection = 0
elif self.keypadup[MENU]:
self.display_page = self.menu.items[self.menu.selection][1]()
elif self.display_page == self.display_info or \
self.display_page == self.display_calibrate_info:
if self.keypadup[MENU]:
self.display_page = self.display_menu
if self.keypadup[UP] or self.keypadup[DOWN]:
self.info_page += 1
elif self.range_edit and self.display_page == self.range_edit.display:
if self.keypadup[MENU]:
self.display_page = self.display_menu
if not self.range_edit.signalk:
self.save_config()
elif updown:
self.range_edit.move(sign*speed*.1)
elif self.display_page == self.display_connecting:
pass # no keys handled for this page
else:
print 'unknown display page', self.display_page
for key in range(len(keynames)):
if self.keypadup[key]:
self.keypad[key] = self.keypadup[key] = False
def key(self, k, down):
if k >= 0 and k < len(self.pins):
if down:
self.keypad[k] = True
else:
self.keypadup[k] = True
def glutkeydown(self, k, x, y):
self.glutkey(k);
def glutkeyup(self, k, x, y):
self.glutkey(k, False)
def glutkey(self, k, down=True):
if k == 'q' or k == 27:
exit(0)
if k == ' ':
key = keynames['auto']
elif k == '\n':
key = keynames['menu']
elif k == '\t':
key = keynames['select']
else:
key = ord(k) - ord('1')
self.key(key, down)
def glutspecialdown(self, k, x, y):
self.glutspecial(k);
def glutspecialup(self, k, x, y):
self.glutspecial(k, False)
def glutspecial(self, k, down=True):
if k == glut.GLUT_KEY_UP:
self.key(keynames['up'], down)
elif k == glut.GLUT_KEY_DOWN:
self.key(keynames['down'], down)
elif k == glut.GLUT_KEY_LEFT:
self.key(keynames['left'], down)
elif k == glut.GLUT_KEY_RIGHT:
self.key(keynames['right'], down)
def idle(self):
self.get('ap.heading')
if any(self.keypadup):
self.longsleep = 0
if any(self.keypad):
self.longsleep += 1
else:
self.longsleep += 10
while self.longsleep > 20:
dt = self.frameperiod / 10.0
time.sleep(dt)
self.longsleep -= 1
# read from keys
for pini in range(len(self.pins)):
pin = self.pins[pini]
value = True
if False:
f = open('/sys/class/gpio/gpio%d/value' % pin)
a = f.readline()
value = bool(int(a))
else:
if GPIO:
value = GPIO.input(pin)
if not value and self.keypad[pini] > 0:
self.keypad[pini] += 1
if pini in self.keystate and self.keystate[pini] != value:
if value:
self.keypadup[pini] = True
else:
self.keypad[pini] = 1
self.keystate[pini] = value
if LIRC:
if self.lirctime and time.time()- self.lirctime > .35:
self.keypad[self.lirckey] = 0
self.keypadup[self.lirckey] = True
self.lirctime = False
#print 'keypad', self.keypad, self.keypadup
while True:
code = LIRC.nextcode(1)
if not code:
break
repeat = code[0]['repeat']
lirc_mapping = {'up': UP, 'down': DOWN, 'left': LEFT, 'right': RIGHT,
'power': AUTO, 'select': SELECT, 'mute': MENU, 'tab': MENU}
code = code[0]['config']
if not code in lirc_mapping:
continue
pini = lirc_mapping[code]
if not self.surface and (pini == MENU or pini == SELECT):
continue
if repeat == 1: # ignore first repeat
#if self.lirctime:
# self.keypad[self.lirckey] = self.keypadup[self.lirckey] = False
pass
else:
if repeat == 0:
if self.lirctime:
self.keypad[self.lirckey] = 0
self.keypadup[self.lirckey] = True
self.keypad[pini] = 0
self.lirckey = pini;
self.keypad[pini] += 1
self.lirctime = time.time()
self.process_keys()
while True:
result = False
if not self.client:
self.connect()
break
try:
result = self.client.receive_single()
except Exception as e:
print 'disconnected', e
self.client = False
if not result:
break
name, data = result
#print name, ' = ', data,
if 'value' in data:
self.last_msg[name] = data['value']
for token in ['min', 'max']:
if token in data:
#print 'name', name, token, ' = ', data[token]
if not name in self.value_list:
self.value_list[name] = {}
self.value_list[name][token] = data[token]
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
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase):
ID_MESSAGES = 1000
ID_CALIBRATE_SERVO = 1001
ID_HEADING_OFFSET = 1002
ID_REQUEST_MSG = 1003
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.signalk_heading_offset)), id = self.ID_HEADING_OFFSET)
self.have_rudder = False
self.request_msg_timer = wx.Timer(self, self.ID_REQUEST_MSG)
self.Bind(wx.EVT_TIMER, self.request_msg, id=self.ID_REQUEST_MSG)
self.request_msg_timer.Start(250)
self.fusionQPose = [1, 0, 0, 0]
self.controltimes = {}
self.settings = {}
def set_watches(self, client):
if not client:
return
watchlist = [
['imu.fusionQPose', 'imu.alignmentCounter', 'imu.heading',
'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading_offset'],
['imu.accel.calibration', 'imu.accel.calibration.age', 'imu.accel',
'imu.accel.calibration.sigmapoints', 'imu.accel.calibration.locked'],
['imu.fusionQPose', 'imu.compass.calibration', 'imu.compass.calibration.age', 'imu.compass',
'imu.compass.calibration.sigmapoints', 'imu.compass.calibration.locked'],
['rudder.offset', 'rudder.scale', 'rudder.nonlinearity',
'rudder.range', 'rudder.calibration_state']]
watchlist.append(list(self.settings))
pageindex = 0
for pagelist in watchlist:
for name in pagelist:
client.watch(name, False)
pageindex = self.m_notebook.GetSelection()
for name in watchlist[pageindex]:
client.watch(name)
def on_con(self, client):
values = client.list_values()
if not self.settings:
fgSettings = wx.FlexGridSizer( 0, 3, 0, 0 )
fgSettings.AddGrowableCol( 1 )
fgSettings.SetFlexibleDirection( wx.BOTH )
fgSettings.SetNonFlexibleGrowMode( wx.FLEX_GROWMODE_SPECIFIED )
self.m_pSettings.SetSizer( fgSettings )
self.m_pSettings.Layout()
fgSettings.Fit( self.m_pSettings )
lvalues = list(values)
lvalues.sort()
for name in lvalues:
if 'units' in values[name]:
v = values[name]
def proc():
s = wx.SpinCtrlDouble(self.m_pSettings, wx.ID_ANY)
s.SetRange(v['min'], v['max'])
s.SetIncrement(min(1, (v['max'] - v['min']) / 100.0))
s.SetDigits(-math.log(s.GetIncrement()) / math.log(10) + 1)
self.settings[name] = s
fgSettings.Add(wx.StaticText(self.m_pSettings, wx.ID_ANY, name), 0, wx.ALL, 5)
fgSettings.Add(s, 0, wx.ALL | wx.EXPAND, 5)
fgSettings.Add(wx.StaticText(self.m_pSettings, wx.ID_ANY, v['units']), 0, wx.ALL, 5)
sname = name
def onspin(event):
self.client.set(sname, s.GetValue())
s.Bind( wx.EVT_SPINCTRLDOUBLE, onspin )
proc()
fgSettings.Add( ( 0, 0), 1, wx.EXPAND, 5 )
fgSettings.Add( ( 0, 0), 1, wx.EXPAND, 5 )
b = wx.Button( self.m_pSettings, wx.ID_OK )
fgSettings.Add ( b, 1, wx.ALIGN_RIGHT, 5)
self.set_watches(client)
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 request_msg(self, event):
if not self.client:
return
page_gets = [[], [], [], ['rudder.angle'], []]
i = self.m_notebook.GetSelection()
for name in page_gets[i]:
self.client.get(name)
def UpdateControl(self, control, update):
t = time.time()
if not control in self.controltimes or t - self.controltimes[control] > .5:
update()
self.controltimes[control] = t
def UpdateLabel(self, label, value):
self.UpdateControl(label, lambda : label.SetLabel(str(value)))
def UpdatedSpin(self, dspin, value):
self.UpdateControl(dspin, lambda : dspin.SetValue(value))
def receive_message(self, msg):
name, data = msg
value = data['value']
if self.m_notebook.GetSelection() == 0:
if name == 'imu.alignmentQ':
self.stAlignment.SetLabel(str(round3(value)) + ' ' + str(math.degrees(pypilot.quaternion.angle(value))))
elif name == 'imu.fusionQPose':
if not value:
return # no imu! show warning?
if self.cCoords.GetSelection() == 1:
self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, self.fusionQPose)
self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, pypilot.quaternion.conjugate(value))
elif self.cCoords.GetSelection() == 2:
ang = pypilot.quaternion.toeuler(self.fusionQPose)[2] - pypilot.quaternion.toeuler(value)[2]
self.boat_plot.Q = pypilot.quaternion.multiply(self.boat_plot.Q, pypilot.quaternion.angvec2quat(ang, [0, 0, 1]))
self.fusionQPose = value
self.BoatPlot.Refresh()
elif name=='imu.alignmentCounter':
self.gAlignment.SetValue(100 - value)
enable = value == 0
self.bLevel.Enable(enable)
elif name == 'imu.pitch':
self.stPitch.SetLabel(str(round3(value)))
elif name == 'imu.roll':
self.stRoll.SetLabel(str(round3(value)))
elif name == 'imu.heel':
self.stHeel.SetLabel(str(round3(value)))
elif name == 'imu.heading':
self.stHeading.SetLabel(str(round3(value)))
elif name == 'imu.heading_offset':
self.signalk_heading_offset = value
self.heading_offset_timer.Start(1000, True)
elif self.m_notebook.GetSelection() == 1:
self.accel_calibration_plot.read_data(msg)
if name == 'imu.accel':
self.AccelCalibration.Refresh()
elif name == 'imu.accel.calibration':
self.stAccelCal.SetLabel(str(round3(value)))
elif name == 'imu.accel.calibration.age':
self.stAccelCalAge.SetLabel(str(value))
elif name == 'imu.accel.calibration.locked':
self.cbAccelCalibrationLocked.SetValue(value)
elif self.m_notebook.GetSelection() == 2:
self.compass_calibration_plot.read_data(msg)
if name == 'imu.compass':
self.CompassCalibration.Refresh()
elif name == 'imu.compass.calibration':
self.stCompassCal.SetLabel(str(round3(value[0])))
elif name == 'imu.compass.calibration.age':
self.stCompassCalAge.SetLabel(str(value))
elif name == 'imu.compass.calibration.locked':
self.cbCompassCalibrationLocked.SetValue(value)
elif self.m_notebook.GetSelection() == 3:
if name == 'rudder.angle':
self.UpdateLabel(self.stRudderAngle, str(round3(value)))
self.have_rudder = type(value) != type(bool)
elif name == 'rudder.offset':
self.UpdateLabel(self.stRudderOffset, str(round3(value)))
elif name == 'rudder.scale':
self.UpdateLabel(self.stRudderScale, (str(round3(value))))
elif name == 'rudder.nonlinearity':
self.UpdateLabel(self.stRudderNonlinearity, str(round3(value)))
elif name == 'rudder.range':
self.UpdatedSpin(self.sRudderRange, value)
elif self.m_notebook.GetSelection() == 4:
for n in self.settings:
if name == n:
self.UpdatedSpin(self.settings[name], value)
break
def servo_console(self, text):
self.stServoCalibrationConsole.SetLabel(self.stServoCalibrationConsole.GetLabel() + text + '\n')
def PageChanged( self, event ):
self.set_watches(self.client)
def onKeyPressAccel( self, event ):
self.onKeyPress(event, self.compass_calibration_plot)
def onKeyPressCompass( self, event ):
self.onKeyPress(event, self.compass_calibration_plot)
def onKeyPress( self, event, plot ):
signalk.scope_wx.wxglutkeypress(event, plot.special, plot.key)
def onClearAccel( self, event ):
self.accel_calibration_plot.points = []
def onClearCompass( self, event ):
self.compass_calibration_plot.points = []
def onAccelCalibrationLocked( self, event ):
self.client.set('imu.accel.calibration.locked', self.cbAccelCalibrationLocked.GetValue())
def onCompassCalibrationLocked( self, event ):
self.client.set('imu.compass.calibration.locked', self.cbCompassCalibrationLocked.GetValue())
def onCalibrationLocked( self, sensor, ctrl ):
self.client.set('imu.'+sensor+'.calibration.locked', self.ctrl.GetValue())
def onMouseEventsAccel( self, event ):
self.AccelCalibration.SetFocus()
self.onMouseEvents( event, self.AccelCalibration, self.accel_calibration_plot )
def onMouseEventsCompass( self, event ):
self.CompassCalibration.SetFocus()
self.onMouseEvents( event, self.CompassCalibration, self.compass_calibration_plot )
def onMouseEvents( self, event, canvas, plot ):
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \
pos[1] - self.lastmouse[1])
canvas.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
canvas.Refresh()
while rotation > 0:
plot.userscale /= .9
rotation -= 1
while rotation < 0:
plot.userscale *= .9
rotation += 1
def onPaintGLAccel( self, event ):
self.onPaintGL( self.AccelCalibration, self.accel_calibration_plot, self.accel_calibration_glContext )
def onPaintGLCompass( self, event ):
self.onPaintGL( self.CompassCalibration, self.compass_calibration_plot, self.compass_calibration_glContext )
def onPaintGL( self, canvas, plot, context ):
wx.PaintDC( canvas )
canvas.SetCurrent(context)
plot.display()
canvas.SwapBuffers()
def onSizeGLAccel( self, event ):
self.accel_calibration_plot.reshape(event.GetSize().x, event.GetSize().y)
def onSizeGLCompass( self, event ):
self.compass_calibration_plot.reshape(event.GetSize().x, event.GetSize().y)
def onResetAlignment(self, event):
self.client.set('imu.alignmentQ', [1, 0, 0, 0])
def onLevel( self, event ):
self.client.set('imu.alignmentCounter', 100)
def onIMUHeadingOffset( self, event ):
self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue())
self.heading_offset_timer.Stop()
def onKeyPressBoatPlot( self, event ):
self.BoatPlot.SetFocus()
k = '%c' % (event.GetKeyCode()&255)
if not event.GetModifiers() & wx.MOD_SHIFT:
k = k.lower()
self.BoatPlot.Refresh()
def onMouseEventsBoatPlot( self, event ):
self.BoatPlot.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1]
q = pypilot.quaternion.angvec2quat((dx**2 + dy**2)**.4/180*math.pi, [dy, dx, 0])
self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q)
self.BoatPlot.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
self.BoatPlot.Refresh()
while rotation > 0:
self.boat_plot.Scale /= .9
rotation -= 1
while rotation < 0:
self.boat_plot.Scale *= .9
rotation += 1
def onPaintGLBoatPlot( self, event ):
wx.PaintDC( self.BoatPlot )
self.BoatPlot.SetCurrent(self.boat_plot_glContext)
# stupid hack
self.boat_plot.reshape(self.BoatPlot.GetSize().x, self.BoatPlot.GetSize().y)
self.boat_plot.display(self.fusionQPose)
self.BoatPlot.SwapBuffers()
def onSizeGLBoatPlot( self, event ):
self.boat_plot.reshape(event.GetSize().x, event.GetSize().y)
self.BoatPlot.Refresh()
def onTextureCompass( self, event ):
self.boat_plot.texture_compass = event.IsChecked()
self.BoatPlot.Refresh()
def onIMUScope( self, event ):
host, port = self.client.host_port
args = ['python', os.path.abspath(os.path.dirname(__file__)) + '/../signalk/scope_wx.py', host + ':' + str(port),
'imu.pitch', 'imu.roll', 'imu.heel', 'imu.heading']
subprocess.Popen(args)
def onRudderResetCalibration( self, event ):
self.client.set('rudder.calibration_state', 'reset')
def onRudderCentered( self, event ):
self.client.set('rudder.calibration_state', 'centered')
def onRudderStarboardRange( self, event ):
self.client.set('rudder.calibration_state', 'starboard range')
def onRudderPortRange( self, event ):
self.client.set('rudder.calibration_state', 'port range')
def onRudderRange( self, event ):
self.client.set('rudder.range', event.GetValue())
class RFWebConfig(Namespace):
def __init__(self, name):
super(Namespace, self).__init__(name)
socketio.start_background_task(target=self.background_thread)
self.client = False
self.last_values = {}
self.last_code = False
self.last_code_time = time.time()
self.servo_timeout = time.time() + 1
self.actions = [RFActionEngage(self),
RFActionSignalK(self, 'disengage', 'ap.enabled', False),
RFActionHeading(self, 1),
RFActionHeading(self, -1),
RFActionHeading(self, 2),
RFActionHeading(self, -2),
RFActionHeading(self, 10),
RFActionHeading(self, -10),
RFActionSignalK(self, 'compassmode', 'ap.mode', 'compass'),
RFActionSignalK(self, 'gpsmode', 'ap.mode', 'gps'),
RFActionSignalK(self, 'windmode', 'ap.mode', 'wind'),
RFActionTack(self, 'tackport', 'port'),
RFActionTack(self, 'tackstarboard', 'starboard')]
self.read_config()
def read_config(self):
config = {}
try:
file = open(config_path)
config = json.loads(file.read())
file.close()
except:
pass
for action in self.actions:
if action.name in config:
action.codes = config[action.name]
def write_config(self):
config = {}
for action in self.actions:
config[action.name] = action.codes
try:
file = open(config_path, 'w')
file.write(json.dumps(config) + '\n')
except IOError:
print('failed to save config file:', self.configfilename)
def on_ping(self):
emit('pong')
def on_codes(self, command):
if not self.last_code:
return
# remove this code from any actions
for action in self.actions:
while self.last_code in action.codes:
action.codes.remove(self.last_code)
# add the last code to the action
for action in self.actions:
if command == action.name:
action.codes.append(self.last_code)
break
self.emit_codes()
self.write_config()
def emit_codes(self):
for action in self.actions:
codes = {'name': action.name, 'codes': map(lambda code : "%X" % code, action.codes)}
socketio.emit('action_codes', codes)
def on_connect(self):
#self.clients[request.sid] = Connection()
if self.client:
socketio.emit('pypilot', 'Connected')
self.emit_codes()
print('Client connected', request.sid)
def on_disconnect(self):
print('Client disconnected', request.sid)
def apply_code(self, code):
socketio.emit('rfcode', "%X" % code)
self.last_code = code
self.last_code_time = time.time()
for action in self.actions:
if code in action.codes:
socketio.emit('action', action.name)
action.trigger()
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()
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)
class CalibrationDialog(autopilot_control_ui.CalibrationDialogBase):
ID_MESSAGES = 1000
ID_CALIBRATE_SERVO = 1001
ID_HEADING_OFFSET = 1002
def __init__(self):
super(CalibrationDialog, self).__init__(None)
self.host = ''
if len(sys.argv) > 1:
self.host = sys.argv[1]
self.client = False
self.compass_calibration_plot = compass_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.dsServoMaxCurrent.SetIncrement(.1)
self.dsServoMaxCurrent.SetDigits(1)
self.dsServoMaxCurrent.Bind(wx.EVT_SPINCTRLDOUBLE, self.onMaxCurrent)
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.signalk_heading_offset)),
id=self.ID_HEADING_OFFSET)
self.servo_timer = wx.Timer(self, self.ID_CALIBRATE_SERVO)
self.Bind(wx.EVT_TIMER,
self.calibrate_servo_timer,
id=self.ID_CALIBRATE_SERVO)
self.servoprocess = False
self.alignmentQ = [1, 0, 0, 0]
self.fusionQPose = [1, 0, 0, 0]
def on_con(self, client):
watchlist = ['imu.compass_calibration', 'imu.compass_calibration_age', \
'imu.compass', 'imu.compass_calibration_sigmapoints', \
'imu.compass_calibration_locked', \
'imu.accel', 'imu.fusionQPose', 'imu.alignmentCounter', \
'imu.heading', \
'imu.alignmentQ', 'imu.pitch', 'imu.roll', 'imu.heel', \
'imu.heading_offset',
'servo.calibration', 'servo.max_current']
for name in watchlist:
client.watch(name)
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 receive_message(self, msg):
name, data = msg
value = data['value']
self.compass_calibration_plot.read_data(msg)
if name == 'imu.compass':
self.CompassCalibration.Refresh()
if name == 'imu.compass_calibration':
self.stCompassCal.SetLabel(str(round3(value[0])))
self.stCompassCalDeviation.SetLabel('N/A')
elif name == 'imu.compass_calibration_age':
self.stCompassCalAge.SetLabel(str(value))
elif name == 'imu.compass_calibration_locked':
self.cbCompassCalibrationLocked.SetValue(value)
elif name == 'imu.alignmentQ':
self.alignmentQ = value
self.stAlignment.SetLabel(
str(round3(value)) + ' ' +
str(math.degrees(pypilot.quaternion.angle(self.alignmentQ))))
elif name == 'imu.fusionQPose':
if self.cCoords.GetSelection() == 1:
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q, self.fusionQPose)
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q, pypilot.quaternion.conjugate(value))
elif self.cCoords.GetSelection() == 2:
ang = pypilot.quaternion.toeuler(
self.fusionQPose)[2] - pypilot.quaternion.toeuler(value)[2]
self.boat_plot.Q = pypilot.quaternion.multiply(
self.boat_plot.Q,
pypilot.quaternion.angvec2quat(ang, [0, 0, 1]))
self.fusionQPose = value
self.BoatPlot.Refresh()
elif name == 'imu.alignmentCounter':
self.gAlignment.SetValue(100 - value)
enable = value == 0
self.bLevel.Enable(enable)
elif name == 'imu.pitch':
self.stPitch.SetLabel(str(round3(value)))
elif name == 'imu.roll':
self.stRoll.SetLabel(str(round3(value)))
elif name == 'imu.heel':
self.stHeel.SetLabel(str(round3(value)))
elif name == 'imu.heading':
self.stHeading.SetLabel(str(round3(value)))
elif name == 'imu.heading_offset':
self.signalk_heading_offset = value
self.heading_offset_timer.Start(1000, True)
elif name == 'servo.calibration':
s = ''
for name in value:
s += name + ' = ' + str(value[name]) + '\n'
self.stServoCalibration.SetLabel(s)
self.SetSize(wx.Size(self.GetSize().x + 1, self.GetSize().y))
elif name == 'servo.calibration/console':
self.stServoCalibrationConsole.SetLabel(
self.stServoCalibrationConsole.GetLabel() + value)
elif name == 'servo.max_current':
self.dsServoMaxCurrent.SetValue(round3(value))
def servo_console(self, text):
self.stServoCalibrationConsole.SetLabel(
self.stServoCalibrationConsole.GetLabel() + text + '\n')
def calibrate_servo_timer(self, event):
if not self.servoprocess:
return
self.servoprocess.poll()
print 'servotimer', self.servoprocess.returncode, self.servoprocess.returncode == None
if self.servoprocess.returncode == None:
self.servoprocess.communicate()
line = self.servoprocess.stdout.readline()
print 'line', line
if line:
self.servo_console(line)
self.servo_timer.Start(150, True)
else:
self.bCalibrateServo.Enable()
if self.servoprocess.returncode == 0:
file = open('servo_calibration')
calibration = json.loads(file.readline())
self.client.set('servo.calibration', calibration)
self.servo_console('calibration sent.')
else:
self.servo_console('calibration failed.')
self.servoprocess = False
def onKeyPressCompass(self, event):
signalk.scope_wx.wxglutkeypress(event, self.compass_calibration_plot.special, \
self.compass_calibration_plot.key)
def onClearCompass(self, event):
self.compass_calibration_plot.points = []
def onCompassCalibrationLocked(self, event):
self.client.set('imu.compass_calibration_locked',
self.cbCompassCalibrationLocked.GetValue())
def onMouseEventsCompass(self, event):
self.CompassCalibration.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
compass_calibration_plot.rotate_mouse(pos[0] - self.lastmouse[0], \
pos[1] - self.lastmouse[1])
self.CompassCalibration.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
self.CompassCalibration.Refresh()
while rotation > 0:
self.compass_calibration_plot.userscale /= .9
rotation -= 1
while rotation < 0:
self.compass_calibration_plot.userscale *= .9
rotation += 1
def onPaintGLCompass(self, event):
wx.PaintDC(self.CompassCalibration)
self.CompassCalibration.SetCurrent(self.compass_calibration_glContext)
self.compass_calibration_plot.display()
self.CompassCalibration.SwapBuffers()
def onSizeGLCompass(self, event):
self.compass_calibration_plot.reshape(event.GetSize().x,
event.GetSize().y)
def StartAlignment(self):
self.client.set('imu.alignmentCounter', 100)
def onResetAlignment(self, event):
self.client.set('imu.alignmentQ', [1, 0, 0, 0])
def onLevel(self, event):
self.StartAlignment()
def onIMUHeadingOffset(self, event):
self.client.set('imu.heading_offset', self.sHeadingOffset.GetValue())
self.heading_offset_timer.Stop()
def onKeyPressBoatPlot(self, event):
self.BoatPlot.SetFocus()
k = '%c' % (event.GetKeyCode() & 255)
if not event.GetModifiers() & wx.MOD_SHIFT:
k = k.lower()
self.BoatPlot.Refresh()
def onMouseEventsBoatPlot(self, event):
self.BoatPlot.SetFocus()
pos = event.GetPosition()
if event.LeftDown():
self.lastmouse = pos
if event.Dragging():
dx, dy = pos[0] - self.lastmouse[0], pos[1] - self.lastmouse[1]
q = pypilot.quaternion.angvec2quat(
(dx**2 + dy**2)**.4 / 180 * math.pi, [dy, dx, 0])
self.boat_plot.Q = pypilot.quaternion.multiply(q, self.boat_plot.Q)
self.BoatPlot.Refresh()
self.lastmouse = pos
rotation = event.GetWheelRotation() / 60
if rotation:
self.BoatPlot.Refresh()
while rotation > 0:
self.boat_plot.Scale /= .9
rotation -= 1
while rotation < 0:
self.boat_plot.Scale *= .9
rotation += 1
def onPaintGLBoatPlot(self, event):
wx.PaintDC(self.CompassCalibration)
self.BoatPlot.SetCurrent(self.boat_plot_glContext)
# stupid hack
self.boat_plot.reshape(self.BoatPlot.GetSize().x,
self.BoatPlot.GetSize().y)
self.boat_plot.display(self.fusionQPose)
self.BoatPlot.SwapBuffers()
def onSizeGLBoatPlot(self, event):
self.boat_plot.reshape(event.GetSize().x, event.GetSize().y)
self.BoatPlot.Refresh()
def onTextureCompass(self, event):
self.boat_plot.texture_compass = event.IsChecked()
self.BoatPlot.Refresh()
def onIMUScope(self, event):
host, port = self.client.host_port
args = [
'python',
os.path.abspath(os.path.dirname(__file__)) +
'/../signalk/scope_wx.py', host + ':' + str(port), 'imu.pitch',
'imu.roll', 'imu.heel', 'imu.heading'
]
subprocess.Popen(args)
def onCalibrateServo(self, event):
try:
self.servoprocess = subprocess.Popen(
['python', 'servo_calibration.py', sys.argv[1]],
stdout=subprocess.PIPE)
self.servo_console('executed servo_calibration.py...')
self.servo_timer.Start(150, True)
self.bCalibrateServo.Disable()
except OSError:
self.servo_console('Failed to execute servo_calibration.py.\n')
def onMaxCurrent(self, event):
self.client.set('servo.max_current', event.GetValue())
class RayClient():
def __init__(self):
self.client = False
self.remote_key = 0
self.blinker_counter = 0
self.mode = MODE_STBY
self.last_servo_command = 0
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(SB, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Stand By: 1
GPIO.setup(AU, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Auto: 2
GPIO.setup(P1, GPIO.IN, pull_up_down=GPIO.PUD_UP) # +1: 4
GPIO.setup(P10, GPIO.IN, pull_up_down=GPIO.PUD_UP) # +10: 8
GPIO.setup(M10, GPIO.IN, pull_up_down=GPIO.PUD_UP) # -10: 16
GPIO.setup(M1, GPIO.IN, pull_up_down=GPIO.PUD_UP) # -1: 32
GPIO.setup(BUZZER, GPIO.OUT)
GPIO.setup(BLINKER, GPIO.OUT)
GPIO.output(BLINKER, 0)
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 last_val(self, name):
if name in self.last_msg:
return self.last_msg[name]
return 'N/A'
def set(self, name, value):
if self.client:
self.client.set(name, value)
def get(self, name):
if self.client:
self.client.get(name)
def bell(self, b):
bell_server = self.last_val('ap.bell_server')
print("bell_server=" + str(bell_server))
if (bell_server != 'N/A'):
if (b == 1):
file = '1bells.wav'
if (b == 2):
file = '2bells.wav'
try:
os.system('echo ' + file + ' | nc -w 1 ' + bell_server +
' 7000')
except Exception as e:
print('ex', e)
self.last_bell = datetime.now()
def beep(self, b):
if (b == 1):
GPIO.output(BUZZER, 1)
time.sleep(0.05)
GPIO.output(BUZZER, 0)
if (b == 2):
GPIO.output(BUZZER, 1)
time.sleep(0.1)
GPIO.output(BUZZER, 0)
if (b == 3):
self.beep(1)
time.sleep(0.05)
self.beep(1)
if (b == 4):
self.beep(3)
time.sleep(0.1)
self.beep(3)
def adjust_gain(self, mode, factor):
if (mode == MODE_P):
gain = "P"
if (mode == MODE_I):
gain = "I"
if (mode == MODE_D):
gain = "D"
gain_name = "ap.pilot." + self.last_val("ap.pilot") + "." + gain
gain_name = gain_name.replace("pilot..", "")
current_gain = self.last_val(gain_name)
new_gain = current_gain * factor
print gain_name + " = " + str(current_gain) + " * " + str(
factor) + " = " + str(new_gain)
SetSignalkValue(gain_name, new_gain)
def adjust_heading(self, adjustment):
name = "ap.heading_command"
current_value = self.last_val(name)
if current_value == "N/A":
current_value = 0
new_value = current_value + adjustment
print name + " = " + str(current_value) + " + " + str(
adjustment) + " = " + str(new_value)
SetSignalkValue(name, new_value)
def doBlinker(self):
if (self.blinker_counter != 1000):
ap_enabled = self.last_val("ap.enabled")
ap_mode = self.last_val("ap.mode")
if (ap_enabled and ap_mode == 'compass' and self.mode not in [
MODE_P, MODE_I, MODE_D, MODE_GAINS, MODE_WAYPOINT_L,
MODE_WAYPOINT_R
]):
self.mode = MODE_AUTO
if (ap_enabled and ap_mode == 'gps' and self.mode not in [
MODE_P, MODE_I, MODE_D, MODE_GAINS, MODE_WAYPOINT_L,
MODE_WAYPOINT_R
]):
self.mode = MODE_TRACK
if (not ap_enabled):
self.mode = MODE_STBY
if (self.mode == MODE_STBY):
light_on = (self.blinker_counter in [1, 2])
if (self.mode == MODE_AUTO):
light_on = (self.blinker_counter not in [1, 2])
if (self.mode == MODE_TRACK):
light_on = (self.blinker_counter not in [1, 2, 5, 6])
if (self.mode == MODE_GAINS):
light_on = (self.blinker_counter % 6 > 3)
if (self.mode in [MODE_P, MODE_I, MODE_D]):
light_on = (self.blinker_counter
not in [1, 2, 11, 12, 21, 22, 31, 32])
if (self.mode in [MODE_WAYPOINT_L, MODE_WAYPOINT_R]):
light_on = (self.blinker_counter % 10 > 5)
if ((datetime.now() - self.last_bell).total_seconds() > 5):
if self.mode in [MODE_WAYPOINT_R]:
self.bell(1)
self.beep(3)
else:
self.bell(2)
self.beep(4)
if (light_on):
GPIO.output(BLINKER, 1)
else:
GPIO.output(BLINKER, 0)
self.blinker_counter = (self.blinker_counter + 1) % 40
def getMessages(self):
# Listen out for SignalK messages; if they arrive, update them in self.last_msg dictionary
while True:
result = False
if not self.client:
self.connect()
break
try:
result = self.client.receive_single()
except Exception as e:
print('disconnected', e)
self.client = False
if not result:
break
name, data = result
if 'value' in data:
self.last_msg[name] = data['value']
#print(str(name) + " = " + str(data['value']))
def handleKey(self, key):
print("key = " + str(key) + ", mode = " + str(self.mode))
next_mode = self.mode
# Standby key
if (key == 1):
if (self.mode in [
MODE_AUTO, MODE_P, MODE_I, MODE_D, MODE_TRACK,
MODE_WAYPOINT_L, MODE_WAYPOINT_R
]):
print "Stand by"
self.set("ap.enabled", False)
self.set("servo.command", 0)
next_mode = MODE_STBY
self.beep(2)
if (self.mode == MODE_GAINS):
next_mode = MODE_D
print "Enter D:"
# Auto key
if (key == 2 and self.mode != MODE_AUTO):
self.beep(1)
print "Auto"
print datetime.now()
self.set("ap.heading_command", int(self.last_val("ap.heading")))
self.set("ap.enabled", True)
self.set("ap.mode", "compass")
print datetime.now()
next_mode = MODE_AUTO
# +1
if (key == 4):
self.beep(1)
if (self.mode == MODE_AUTO):
print "+1"
self.adjust_heading(+1)
if (self.mode in [MODE_P, MODE_I, MODE_D]):
self.adjust_gain(self.mode, FACTOR_LOW)
if (self.mode in [MODE_STBY]):
servo_command = -20
SetSignalkValue("servo.speed.max", SERVO_SPEED_SLOW)
SetSignalkValue("servo.speed.min", SERVO_SPEED_SLOW)
SetSignalkValue("servo.command", servo_command)
self.last_servo_command = servo_command
# -1
if (key == 32):
self.beep(1)
if (self.mode == MODE_AUTO):
print "-1"
self.adjust_heading(-1)
if (self.mode == MODE_GAINS):
next_mode = MODE_P
print "Enter P:"
if (self.mode in [MODE_P, MODE_I, MODE_D]):
self.adjust_gain(self.mode, 1 / FACTOR_LOW)
if (self.mode in [MODE_STBY]):
servo_command = +20
SetSignalkValue("servo.speed.max", SERVO_SPEED_SLOW)
SetSignalkValue("servo.speed.min", SERVO_SPEED_SLOW)
SetSignalkValue("servo.command", servo_command)
self.last_servo_command = servo_command
# +10
if (key == 8):
self.beep(2)
if (self.mode == MODE_AUTO):
print "+10"
self.adjust_heading(+10)
if (self.mode in [MODE_P, MODE_I, MODE_D]):
self.adjust_gain(self.mode, FACTOR_MEDIUM)
if (self.mode in [MODE_STBY]):
servo_command = -1000
SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST)
SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST)
SetSignalkValue("servo.command", servo_command)
self.last_servo_command = servo_command
# -10
if (key == 16):
self.beep(2)
if (self.mode == MODE_AUTO):
print "-10"
self.adjust_heading(-10)
if (self.mode == MODE_GAINS):
next_mode = MODE_I
print "Enter I:"
if (self.mode in [MODE_P, MODE_I, MODE_D]):
self.adjust_gain(self.mode, 1 / FACTOR_MEDIUM)
if (self.mode in [MODE_STBY]):
servo_command = +1000
SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST)
SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST)
SetSignalkValue("servo.command", servo_command)
self.last_servo_command = servo_command
# Track -10 & +10
if (key == 24 and self.mode
in [MODE_AUTO, MODE_WAYPOINT_L, MODE_WAYPOINT_R]):
self.beep(3)
print "Track"
SetSignalkValue("ap.enabled", True)
SetSignalkValue("ap.mode", "gps")
next_mode = MODE_TRACK
# Tack Port -1 & -10
if (key == 48 and self.mode == MODE_AUTO):
self.beep(3)
print "Tack Port"
adjust_heading(-100)
# SetSignalkValue("ap.tack.direction", "port")
# SetSignalkValue("ap.tack.state", "begin")
# Tack Starboard +1 & +10
if (key == 12 and self.mode == MODE_AUTO):
self.beep(3)
print "Tack Starboard"
adjust_heading(+100)
# SetSignalkValue("ap.tack.direction", "starboard")
# SetSignalkValue("ap.tack.state", "begin")
# Set gains: +1 & -1
if (key == 36 and self.mode
in [MODE_AUTO, MODE_TRACK, MODE_P, MODE_I, MODE_D]):
self.beep(3)
print "Choose gain"
next_mode = MODE_GAINS
# Artificial mode: Waypoint Arrival
if (key == 1000 and self.mode in [MODE_TRACK]):
print "Waypoint arrival, confirm with 'Track'"
next_mode = MODE_WAYPOINT_R
self.bell(1)
if (key == 1001 and self.mode in [MODE_TRACK]):
print "Waypoint arrival, confirm with 'Track'"
next_mode = MODE_WAYPOINT_L
self.bell(2)
if self.mode != next_mode:
blinker_counter = 1
self.mode = next_mode
self.remote_key = 0
def processKeys(self):
# wait for a button to be pressed. In the meantime, listen for SignalK messages and blink the LED:
while (GPIO.input(SB) == 1 and GPIO.input(AU) == 1
and GPIO.input(P1) == 1 and GPIO.input(P10) == 1
and GPIO.input(M10) == 1 and GPIO.input(M1) == 1
and self.remote_key == 0):
self.getMessages()
self.doBlinker()
time.sleep(0.05)
try:
with open('/tmp/remote', 'r') as myfile:
line = myfile.read().replace("\n", "")
print "remote=" + line
os.remove('/tmp/remote')
self.remote_key = int(line)
except:
self.remote_key = 0
# wait for a possible second button to be pressed in parallel, or at least for the button to be finished vibrating
time.sleep(0.05)
# store the key (or key combination) in one variable
key = (1 - GPIO.input(SB)) + 2 * (1 - GPIO.input(AU)) + 4 * (
1 - GPIO.input(P1)) + 8 * (1 - GPIO.input(P10)) + 16 * (
1 - GPIO.input(M10)) + 32 * (1 -
GPIO.input(M1)) + self.remote_key
self.handleKey(key)
# Wait for key to be lifted
while (GPIO.input(SB) == 0 or GPIO.input(AU) == 0
or GPIO.input(P1) == 0 or GPIO.input(P10) == 0
or GPIO.input(M10) == 0 or GPIO.input(M1) == 0):
self.doBlinker()
time.sleep(0.05)
if key in [4, 8, 16, 32] and self.mode in [MODE_STBY]:
SetSignalkValue("servo.command", self.last_servo_command)
# Key released
SetSignalkValue("servo.speed.max", SERVO_SPEED_FAST)
SetSignalkValue("servo.speed.min", SERVO_SPEED_FAST)
# Immediately stop manual movement:
if (self.mode in [MODE_STBY]):
SetSignalkValue("servo.command", 0)
