def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-p", "--port",
help="path for serial port")
args = vars(ap.parse_args())
# inits IMU
port = args.get('port', None)
if port is None:
port = '/dev/ttyUSB_IMU'
imu = IMU(port)
# start subprocess
imu.start()
try:
while True:
print("yaw = ", imu.get_yaw())
print("yaw2 = ", imu.get_yaw2())
print("pitch = ", imu.get_pitch())
print("roll = ", imu.get_roll())
print("angx = ", imu.get_angx())
print("angy = ", imu.get_angy())
print("angz = ", imu.get_angz())
print("accx = ", imu.get_accx())
print("accy = ", imu.get_accy())
print("accz = ", imu.get_accz())
print("magx = ", imu.get_magx())
print("magy = ", imu.get_magy())
print("magz = ", imu.get_magz())
print()
time.sleep(1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-s", "--speed")
args = vars(ap.parse_args())
set_speed = args.get('speed', 0)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# inits CV
cv = CVManager(
0, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333)
cv.add_core("Depth", DepthCore(), True)
# start subprocess
imu.start()
mcu.start()
cv.start()
mcu.wait()
pidR = pidRoll(1, 0.2, 0) # 1, 0 , 0
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.4, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
while True:
counter += 1
if not cv.get_result('Depth')[0] is None:
depth = 150 - cv.get_result('Depth')[0] * 5
else:
depth = 70
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2()
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(100 - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0]
motor_fr = sentValues[1]
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4]
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 5 == 0:
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
mcu.stop()
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-s", "--speed")
args = vars(ap.parse_args())
set_speed = args.get('speed', 0)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
imu.start()
mcu.start()
mcu.wait()
start_time = time.time()
depth_speed = 0
pidR = pidRoll(0.4, 0, 0) # 1, 0 , 0
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.4, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
while True:
counter += 1
depth = mcu.get_depth()
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2()
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(70 - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
if abs((time.time() - start_time) % 5) < 1:
depth_speed = 0.4
else:
depth_speed = 0
motor_fl = sentValues[0] + depth_speed
motor_fr = sentValues[1] + depth_speed
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4] + depth_speed
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 5 == 0:
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
mcu.stop()
class GCS(msppg.Parser):
def __init__(self):
msppg.Parser.__init__(self)
# No communications or arming yet
self.comms = None
self.armed = False
self.gotimu = False
# Do basic Tk initialization
self.root = tk.Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' %
(DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = tk.Frame(self.root)
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w,
tk.PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1,
self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2,
self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2,
self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2,
self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = tk.StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = tk.Canvas(self.root,
width=DISPLAY_WIDTH,
height=DISPLAY_HEIGHT,
background='black')
self.canvas.pack()
# Set up a text label for reporting errors
errmsg = 'No response from board. Possible reasons:\n\n' + \
' * You connected to the wrong port.\n\n' + \
' * Firmware uses serial receiver\n' + \
' (DSMX, SBUS), but receiver is\n' + \
' not connected.'
self.error_label = tk.Label(self.canvas,
text=errmsg,
bg='black',
fg='red',
font=(None, 24),
justify=tk.LEFT)
self.hide(self.error_label)
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
self._show_splash()
# Create a message parser
#self.parser = msppg.Parser()
# Set up parser's request strings
self.attitude_request = msppg.serialize_ATTITUDE_RADIANS_Request()
self.rc_request = msppg.serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = [0] * 3
self.rxchannels = [0] * 6
# A hack to support display in IMU dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def handle_RC_NORMAL(self, c1, c2, c3, c4, c5, c6):
# Display throttle as [0,1], other channels as [-1,+1]
self.rxchannels = c1 / 2. + .5, c2, c3, c4, c5, c6
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def handle_ATTITUDE_RADIANS(self, x, y, z):
self.roll_pitch_yaw = x, -y, z
self.gotimu = True
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _add_pane(self):
pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=tk.BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = tk.Button(parent, text=label, command=callback)
button.pack(side=tk.LEFT)
button.config(state='disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self._send_attitude_request()
self.imu.start()
def _start(self):
self._send_attitude_request()
self.imu.start()
self.gotimu = False
self.hide(self.error_label)
self.scheduleTask(CONNECTION_DELAY_MSEC, self._checkimu)
def _checkimu(self):
if not self.gotimu:
self.imu.stop()
self.error_label.place(x=50, y=50)
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(tk.ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'usbmodem' in portname or 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
if not portname in ['COM1', 'COM2']:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
*ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=tk.LEFT)
# Disconnected
if ports == []:
self.portsmenu['menu'].delete(0, 'end')
self.portsvar.set('')
self._disable_button(self.button_connect)
self._disable_buttons()
# Connected
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [0] * 4
values[index - 1] = percent / 100.
self.comms.send_message(msppg.serialize_SET_MOTOR_NORMAL, values)
def _show_splash(self):
self.splash = self.canvas.create_image(SPLASH_LOCATION,
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video",
help="path to the (optional) video file")
ap.add_argument("-c", "--camera",
help="index of camera")
ap.add_argument("-o", "--output",
help="path to save the video")
ap.add_argument("-s", "--speed")
ap.add_argument("-t", "--time")
args = vars(ap.parse_args())
if args.get("video", False):
vs = args.get("video", False)
elif args.get("camera", False):
vs = int(args.get("camera", False))
else:
vs = 0
set_speed = float(args.get('speed', 0))
set_time = float(args.get('time', 0))
print('Speed', set_speed)
print('Time', set_time)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
yaw = 0
# inits CV
cv = CVManager(vs, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333, # start stream server at port 3333
delay=5,
outputfolder=args.get("output"))
cv.add_core("GateTracker", GateTrackerV3(), True)
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
cv.start()
imu.start()
mcu.start()
mcu.wait()
time.sleep(3)
start_time = time.time()
depth_speed = 0
pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5
pidP = pidPitch(0.6, 0, 0)# 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.3, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
last_cv_gate = 0
gate_passed = False
while time.time() - start_time < set_time:
counter += 1
gate, _, gate_size = cv.get_result("GateTracker")
depth = mcu.get_depth()
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
if True or gate_passed or gate is None: # just go straight
yaw = imu.get_yaw2(0)
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1)
if gate_size > 350:
gate_passed = True
if abs(yaw) > 10:
speed = 0
else:
speed = set_speed
if abs((time.time() - start_time) % 5) < 1:
depth_speed = 0.45
else:
depth_speed = 0
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(70-depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0] + depth_speed
motor_fr = sentValues[1] + depth_speed
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4] + depth_speed
# Put control codes here
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 20 == 0:
print('Gate', gate)
print('GateSize', gate_size)
print('Passed?', gate_passed)
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f'%(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
mcu.set_motors(0, 0, 0, 0, 0)
print("Stopping remaining threads...")
cv.stop()
imu.stop()
mcu.stop()
class GCS:
def __init__(self):
# No communications or arming yet
self.comms = None
self.armed = False
# Do basic Tk initialization
self.root = Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = Frame(self.root)
self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2, self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
self.canvas.pack()
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = PhotoImage(file='media/splash.gif')
self._show_splash()
# Create a message parser
self.parser = MSP_Parser()
# Set up parser's request strings
self.attitude_request = serialize_ATTITUDE_RADIANS_Request()
self.rc_request = serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = 0,0,0
self.rxchannels = 0,0,0,0,0,0,0,0
# A hack to support display in IMU dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = Button(parent, text=label, command=callback)
button.pack(side=LEFT)
button.config(state = 'disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
def _start(self):
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
self.parser.set_RC_NORMAL_Handler(self._handle_rc)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=LEFT)
if ports == []:
self._disable_button(self.button_connect)
self._disable_buttons()
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [0]*4
values[index-1] = percent / 100.
self.comms.send_message(serialize_SET_MOTOR_NORMAL, values)
def _show_splash(self):
self.splash = self.canvas.create_image((400,250), image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
print(x, y, z)
self.roll_pitch_yaw = x, -y, z
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):
# Display throttle as [0,1], other channels as [-1,+1]
self.rxchannels = c1/2.+.5, c2, c3, c4, c5
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
class GCS(MspParser):
def __init__(self):
MspParser.__init__(self)
# No communications or arming yet
self.comms = None
self.armed = False
self.gotimu = False
# Do basic Tk initialization
self.root = tk.Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' %
(DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = tk.Frame(self.root)
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w,
tk.PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1,
self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2,
self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2,
self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2,
self._receiver_button_callback)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = tk.StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = tk.Canvas(self.root,
width=DISPLAY_WIDTH,
height=DISPLAY_HEIGHT,
background='black')
self.canvas.pack()
# Set up a text label for reporting errors
errmsg = ('No response from board. Possible reasons:\n\n' +
' * You connected to the wrong port.\n\n' +
' * IMU is not responding.\n\n')
self.error_label = tk.Label(self.canvas,
text=errmsg,
bg='black',
fg='red',
font=(None, 24),
justify=tk.LEFT)
self.hide(self.error_label)
# Add widgets for motor-testing dialogs; hide them immediately
self.motors_quadxmw = self._add_motor_dialog(MotorsQuadXMW)
self.motors_coaxial = self._add_motor_dialog(MotorsCoaxial)
# Create receiver dialog
self.receiver = Receiver(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a splash image
self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
self._show_splash()
# Set up parser's request strings
self.attitude_request = MspParser.serialize_ATTITUDE_Request()
self.rc_request = MspParser.serialize_RC_Request()
# No messages yet
self.roll_pitch_yaw = [0] * 3
self.rxchannels = [0] * 6
def quit(self):
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def handle_RC(self, c1, c2, c3, c4, c5, c6):
def norm(x):
MIN = 987
MAX = 2011
return (x - MIN) / (MAX - MIN)
def scale(x):
return 2 * norm(x) - 1
# Scale throttle from [-1,+1] to [0,1]
self.rxchannels = norm(c1), scale(c2), scale(c3), scale(c4), scale(
c5), scale(c6)
# As soon as we handle the callback from one request, send another
# request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
def handle_ATTITUDE(self, angx, angy, heading):
self.roll_pitch_yaw = rad(angx / 10), -rad(angy / 10), rad(heading)
self.gotimu = True
# As soon as we handle the callback from one request, send another
# request, if receiver dialog is running
if self.imu.running:
self._send_attitude_request()
def _add_pane(self):
pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=tk.BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = tk.Button(parent, text=label, command=callback)
button.pack(side=tk.LEFT)
button.config(state=('disabled' if disabled else 'normal'))
return button
def _add_motor_dialog(self, dialog):
d = dialog(self)
d.stop()
return d
# unsigned int => signed float conversion
def _float(self, intval):
return intval / 1000 - 2
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.receiver.stop()
self._send_attitude_request()
self.imu.start()
def _start(self):
self._send_attitude_request()
self.imu.start()
self.gotimu = False
self.hide(self.error_label)
self.scheduleTask(CONNECTION_DELAY_MSEC, self._checkimu)
def _checkimu(self):
if not self.gotimu:
self.imu.stop()
self.error_label.place(x=50, y=50)
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
# Sends state request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
# self.comms.send_request(self.actuator_type_request)
self.imu.stop()
self.receiver.stop()
self.motors_quadxmw.start()
def _clear(self):
self.canvas.delete(tk.ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.receiver.stop()
if self.comms is not None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if any(name in portname
for name in ('usbmodem', 'ttyACM', 'ttyUSB', 'COM')):
if portname not in ('COM1', 'COM2'):
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
*ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=tk.LEFT)
# Disconnected
if ports == []:
self.portsmenu['menu'].delete(0, 'end')
self.portsvar.set('')
self._disable_button(self.button_connect)
self._disable_buttons()
# Connected
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, motors):
if self.comms is not None: # Guard at startup
self.comms.send_message(
MspParser.serialize_SET_MOTOR,
(motors[0], motors[1], motors[2], motors[3]))
def _show_splash(self):
self.splash = self.canvas.create_image(SPLASH_LOCATION,
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_arm_status(self, armed):
self.armed = armed
def _handle_battery_status(self, volts, amps):
return
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-o", "--output")
ap.add_argument("-s", "--speed")
ap.add_argument("-t", "--time") # time to turn after passing
ap.add_argument("-ft",
"--forceturn") # time to force turn after launching, 30
ap.add_argument("-a", "--angle")
ap.add_argument("-d", "--depth") # 80
args = vars(ap.parse_args())
set_speed = float(args.get('speed', 0))
set_depth = float(args.get('depth', 0))
time_after_passing = float(args.get('time', 0))
time_force_turn = float(args.get('forceturn', 0))
angle_to_turn = float(args.get('angle', 0))
# inits CV
cv = CVManager(
0, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333, # start stream server at port 3333
delay=5,
outputfolder=args.get("output"))
cv.add_core("GateTracker", GateTrackerV3(), False)
cv.add_core("Flare", Flare(), False)
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
cv.start()
imu.start()
mcu.start()
imu.reset_yaw2(-angle_to_turn, 2) # for state 3
mcu.wait()
cv.enable_core("GateTracker")
pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(1, 0, 0)
pidY = pidYaw(1, 0.3, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
last_cv_gate = 0
yaw = 0
speed = 0
state = 0
timer_for_state0 = time.time()
timer_for_state1 = 0
timer_for_state2 = 0
# 0 -> go find the gate
# 1 -> continue after passing the gate
# 2 -> find flare
# 3 -> surfacing
while True:
counter += 1
if state == 0:
gate, _, gate_size = cv.get_result("GateTracker")
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
speed = set_speed
if gate is None: # just go straight
yaw = imu.get_yaw2(0) # original heading
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1) # heading with CV
if gate_size > 350:
state = 1
if time.time() - timer_for_state0 > time_force_turn:
state = 1
print('Gate', gate)
print('GateSize', gate_size)
# go straight
elif state == 1:
if timer_for_state1 == 0: # first time
cv.disable_core('GateTracker')
timer_for_state1 = time.time()
elif time.time() - timer_for_state1 > time_after_passing:
state = 2
cv.enable_core('Flare')
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2(0) # original heading
speed = set_speed
# go to the flare
elif state == 2:
gate, _, gate_size = cv.get_result("Flare")
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
speed = set_speed
print(gate, gate_size)
if gate is None: # just go straight
yaw = imu.get_yaw2(2)
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1)
if not gate_size is None:
if gate_size > 200:
timer_for_state2 = time.time()
if timer_for_state2 != 0 and time.time(
) - timer_for_state2 > 10:
state = 3
# surfacing
else:
depth = 500
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = 0
speed = 0
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(set_depth - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0]
motor_fr = sentValues[1]
motor_bl = sentValues[2] + speed
motor_br = sentValues[3] + speed
motor_t = sentValues[4]
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 20 == 0:
print('State:', state)
print('Depth:', depth)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
cv.stop()
imu.stop()
mcu.stop()
class GCS:
def __init__(self):
# No communications or arming yet
self.comms = None
self.armed = False
# Do basic Tk initialization
self.root = Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = Frame(self.root)
self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2, self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
self.canvas.pack()
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = PhotoImage(file='media/splash.gif')
self._show_splash()
# Create a message parser
self.parser = MSP_Parser()
# Set up parser's request strings
self.attitude_request = serialize_ATTITUDE_Request()
self.rc_request = serialize_RC_Request()
# No messages yet
self.roll_pitch_yaw = 0,0,0
self.rxchannels = 0,0,0,0,0
# A hack to support display in IMU dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = Button(parent, text=label, command=callback)
button.pack(side=LEFT)
button.config(state = 'disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
def _start(self):
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
self.parser.set_RC_Handler(self._handle_rc)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=LEFT)
if ports == []:
self._disable_button(self.button_connect)
self._disable_buttons()
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [PWM_MIN]*4
values[index-1] += int(percent/100. * (PWM_MAX-PWM_MIN))
self.comms.send_message(serialize_SET_MOTOR, values)
def _show_splash(self):
self.splash = self.canvas.create_image((400,250), image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
self.roll_pitch_yaw = x, -y, z
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):
self.rxchannels = c1, c2, c3, c4, c5
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return