Esempio n. 1
0
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()
Esempio n. 3
0
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()
Esempio n. 4
0
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
Esempio n. 5
0
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()
Esempio n. 6
0
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
Esempio n. 7
0
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
Esempio n. 8
0
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()
Esempio n. 9
0
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