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 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 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]
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 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()
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)