Esempio n. 1
0
    def __init__(self, options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {
            "MS5611": self.ui.label_baroTest,
            "MPU6050": self.ui.label_MPUTest,
            "HMC5883L": self.ui.label_magTest
        }

        self.state = STATE.DISCONNECTED

        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)

        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(
            lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.
                                                      value() if on else 0))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(
            self.ui.spinBox_pktHZ.value())  # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(
            self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode(
            self.ui.checkBox_xmode.isChecked())

        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(
            self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda: self.connectPressed(
            self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf,
                                         self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)

        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(
            self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)

        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw * 10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(
            lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw / 10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(
            lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.
                                                   value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(
            self.ui.checkBox_yaw.checkState())  # force update
        self.ui.pushButton_north.clicked.connect(
            lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw(
            )))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(
            self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(
            lambda x: self.logManager.setGroundLevel(
                self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(
            self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(
            lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.
                                                          getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)

        # init previous settings
        self.readSettings()

        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(
            lambda x: self.ui.label_up.setText(str(round(x, 2)) + "ms"))
        self.sync.sig_delayDown.connect(
            lambda x: self.ui.label_down.setText(str(round(x, 2)) + "ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(
            self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(
            lambda x: self.ui.label_cputime.setText("%08.03fs" % x))
        self.sync.sig_flieTime.connect(
            lambda x: self.ui.label_flietime.setText("%08.03fs" % x))
        self.sync.sig_diffTime.connect(
            lambda x: self.ui.label_difftime.setText("%08.03fs" % x))

        self.ui.checkBox_rosLog.stateChanged.connect(
            self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(
            lambda x: self.logManager.setPubToRos(
                min(self.ui.groupBox_ros.isChecked(),
                    self.ui.checkBox_rosLog.checkState())))  #

        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000 / self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(
            lambda x: self.guiUpdateTimer.setInterval(1000 / x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(
            lambda x: self.ros.sig_joydata.connect(self.setInputJoy)
            if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)

        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()

        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(
            lambda: self.connectPressed(self.ui.comboBox_connect.currentText(),
                                        auto=False))  # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(
            self.ui.checkBox_kill.checkState())  # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(
            self.ui.checkBox_hover.checkState())  # force update

        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(
            lambda x: self.updateBaroTopics(not x))

        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(
            lambda found: self.ui.label_baroFound.setText("Yes"
                                                          if found else "No"))
        self.paramManager.sig_magFound.connect(
            lambda found: self.ui.label_magFound.setText("Yes"
                                                         if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[
            str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect(lambda v: self.setToUpdate(
            "vbat", self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect(lambda v: self.setToUpdate(
            "cpu", self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn, hz))
        self.flie.outKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut, hz))

        # Connections GUI to GUI

        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)

        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()
Esempio n. 2
0
    def __init__(self,options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {"MS5611": self.ui.label_baroTest, "MPU6050": self.ui.label_MPUTest, "HMC5883L": self.ui.label_magTest}

        self.state = STATE.DISCONNECTED


        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)


        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.value() if on else 0 ))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(self.ui.spinBox_pktHZ.value()) # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode( self.ui.checkBox_xmode.isChecked())


        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf, self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)


        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)


        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw*10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw/10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(self.ui.checkBox_yaw.checkState()) # force update
        self.ui.pushButton_north.clicked.connect(lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw()))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(lambda x: self.logManager.setGroundLevel(self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)




        # init previous settings
        self.readSettings()


        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(lambda  x: self.ui.label_up.setText(str(round(x,2))+"ms"))
        self.sync.sig_delayDown.connect(lambda  x: self.ui.label_down.setText(str(round(x,2))+"ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(lambda  x: self.ui.label_cputime.setText("%08.03fs"%x))
        self.sync.sig_flieTime.connect(lambda  x: self.ui.label_flietime.setText("%08.03fs"%x))
        self.sync.sig_diffTime.connect(lambda  x: self.ui.label_difftime.setText("%08.03fs"%x))


        self.ui.checkBox_rosLog.stateChanged.connect(self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(lambda x: self.logManager.setPubToRos(min(self.ui.groupBox_ros.isChecked(), self.ui.checkBox_rosLog.checkState()))) #




        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000/self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(lambda x: self.guiUpdateTimer.setInterval(1000/x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(lambda x: self.ros.sig_joydata.connect(self.setInputJoy) if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)





        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()


        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=False)) # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(self.ui.checkBox_kill.checkState()) # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(self.ui.checkBox_hover.checkState()) # force update



        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(lambda x: self.updateBaroTopics(not x))






        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(lambda found: self.ui.label_baroFound.setText("Yes" if found else "No"))
        self.paramManager.sig_magFound.connect(lambda found: self.ui.label_magFound.setText("Yes" if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect( lambda v : self.setToUpdate("vbat",self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect( lambda v : self.setToUpdate("cpu",self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn,hz))
        self.flie.outKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut,hz))


        # Connections GUI to GUI


        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)


        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()
Esempio n. 3
0
class DriverWindow(QtGui.QMainWindow):
    """ Main window and application """

    sig_requestScan = pyqtSignal()
    sig_requestConnect = pyqtSignal(str)
    sig_requestDisconnect = pyqtSignal()

    def __init__(self, options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {
            "MS5611": self.ui.label_baroTest,
            "MPU6050": self.ui.label_MPUTest,
            "HMC5883L": self.ui.label_magTest
        }

        self.state = STATE.DISCONNECTED

        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)

        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(
            lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.
                                                      value() if on else 0))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(
            self.ui.spinBox_pktHZ.value())  # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(
            self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode(
            self.ui.checkBox_xmode.isChecked())

        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(
            self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda: self.connectPressed(
            self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf,
                                         self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)

        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(
            self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)

        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw * 10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(
            lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw / 10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(
            lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.
                                                   value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(
            self.ui.checkBox_yaw.checkState())  # force update
        self.ui.pushButton_north.clicked.connect(
            lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw(
            )))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(
            self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(
            lambda x: self.logManager.setGroundLevel(
                self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(
            self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(
            lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.
                                                          getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)

        # init previous settings
        self.readSettings()

        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(
            lambda x: self.ui.label_up.setText(str(round(x, 2)) + "ms"))
        self.sync.sig_delayDown.connect(
            lambda x: self.ui.label_down.setText(str(round(x, 2)) + "ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(
            self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(
            lambda x: self.ui.label_cputime.setText("%08.03fs" % x))
        self.sync.sig_flieTime.connect(
            lambda x: self.ui.label_flietime.setText("%08.03fs" % x))
        self.sync.sig_diffTime.connect(
            lambda x: self.ui.label_difftime.setText("%08.03fs" % x))

        self.ui.checkBox_rosLog.stateChanged.connect(
            self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(
            lambda x: self.logManager.setPubToRos(
                min(self.ui.groupBox_ros.isChecked(),
                    self.ui.checkBox_rosLog.checkState())))  #

        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000 / self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(
            lambda x: self.guiUpdateTimer.setInterval(1000 / x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(
            lambda x: self.ros.sig_joydata.connect(self.setInputJoy)
            if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)

        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()

        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(
            lambda: self.connectPressed(self.ui.comboBox_connect.currentText(),
                                        auto=False))  # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(
            self.ui.checkBox_kill.checkState())  # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(
            self.ui.checkBox_hover.checkState())  # force update

        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(
            lambda x: self.updateBaroTopics(not x))

        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(
            lambda found: self.ui.label_baroFound.setText("Yes"
                                                          if found else "No"))
        self.paramManager.sig_magFound.connect(
            lambda found: self.ui.label_magFound.setText("Yes"
                                                         if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[
            str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect(lambda v: self.setToUpdate(
            "vbat", self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect(lambda v: self.setToUpdate(
            "cpu", self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn, hz))
        self.flie.outKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut, hz))

        # Connections GUI to GUI

        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)

        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()

    @pyqtSlot()
    def updateBaroTopics(self, forceReset=False):
        """ Handles using another baro as a ground station.

        """

        if forceReset:
            if self.ui.pushButton_baro.text() == 'Disconnect':
                print 'Disconnecting from', self.ui.comboBox_baro.currentText()
                self.ros.subBaro('')

            self.ui.comboBox_baro.clear()
            self.ui.comboBox_baro.addItem("None Detected")
            self.ui.pushButton_baro.setText('Refresh')

        else:
            if self.ui.pushButton_baro.text() == 'Connect':
                # Connect
                self.ui.comboBox_baro.setDisabled(True)
                print 'Connecting to', self.ui.comboBox_baro.currentText()
                self.ros.subBaro(self.ui.comboBox_baro.currentText())
                self.ui.pushButton_baro.setText('Disconnect')

            elif self.ui.pushButton_baro.text() == 'Disconnect':
                # Disconnect
                print 'Disconnecting from', self.ui.comboBox_baro.currentText()
                self.ros.subBaro('')
                self.ui.comboBox_baro.clear()
                self.ui.comboBox_baro.addItem("None Detected")
                self.ui.pushButton_baro.setText('Refresh')
                self.ui.comboBox_baro.setDisabled(False)

            else:
                # Scan
                self.ui.comboBox_baro.setDisabled(False)
                self.ui.pushButton_baro.setDisabled(False)
                self.ui.comboBox_baro.clear()
                topics = [
                    t for t in self.ros.getTopics() if t.endswith("/baro")
                    and len(t) >= 6 and t[-6] != str(self.options.radio)
                ]
                if len(topics) > 0:
                    self.ui.comboBox_baro.addItems(topics)
                    self.ui.pushButton_baro.setText('Connect')
                else:
                    self.ui.comboBox_baro.addItem("None Detected")

    @pyqtSlot()
    def updateGui(self):
        """ Execute all funcs in the queue """
        #rospy.loginfo("Draw queue size: %d/%s", len(self.guiUpdateQueue), self.guiUpdateQueueSave)
        for f in self.guiUpdateQueue.values():
            f()
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0

    def setToUpdate(self, name, func, *args):
        """ Add functions to call at gui update hz rate"""
        if self.ui.checkBox_guiHZ.isChecked():
            # Limit rate
            self.guiUpdateQueue[name] = partial(func, *args)
            self.guiUpdateQueueSave += 1
        else:
            #dont limit rate
            f = partial(func, *args)
            f()

    @pyqtSlot(float, float, float, float, bool)
    def setInputJoy(self, r, p, y, t, h):
        #TODO: should only do this at a specific hz
        self.setToUpdate("joyR", self.ui.doubleSpinBox_r.setValue, r)
        self.setToUpdate("joyP", self.ui.doubleSpinBox_p.setValue, p)
        self.setToUpdate("joyY", self.ui.doubleSpinBox_y.setValue, y)
        self.setToUpdate("joyT", self.ui.doubleSpinBox_t.setValue, t)
        self.ui.label_hover.setText("On" if h else "Off")

    @pyqtSlot()
    def genRosMsg(self):
        #TODO: should do this in its own thread, or even as a progress bar
        self.ui.pushButton_genRosMsg.setEnabled(False)
        self.ui.pushButton_genRosMsg.setText("Generating")
        generateRosMessages(self.flie.getLogToC())
        self.ui.pushButton_genRosMsg.setEnabled(True)
        self.ui.pushButton_genRosMsg.setText("Generate ROS msgs from ToC")

    def closeEvent(self, event):
        """ Intercept shutdowns to cleanly disconnect from the flie and cleanly shut ROS down too """
        rospy.loginfo("Close Event")
        #self.autoRetryTimer.stop()

        # Shut Down the Flie
        if self.state < STATE.GEN_DISCONNECTED:
            rospy.loginfo("Triggering flie disconnect for shutdown")
            self.ui.pushButton_connect.setText("Shutting Down...")
            self.flie.requestDisconnect()

        # Clean up ROS
        rospy.signal_shutdown("User closed window")

        # Save State
        self.writeSettings()

        # Commence shutdown
        event.accept()

    def readSettings(self):
        """ Load settings from previous session """
        rospy.logdebug("Loading previous session settings")
        settings = QSettings(
            "omwdunkley", "reset" if self.options.reset else "flieROS" +
            str(self.options.radio))
        self.resize(settings.value("size", QVariant(QSize(300, 500))).toSize())
        self.move(settings.value("pos", QVariant(QPoint(200, 200))).toPoint())

        self.ui.checkBox_reconnect.setChecked(
            settings.value(
                "autoReconnect",
                QVariant(self.ui.checkBox_reconnect.isChecked())).toBool())
        self.ui.checkBox_beep.setChecked(
            settings.value("beepOn", QVariant(
                self.ui.checkBox_beep.isChecked())).toBool())
        self.ui.checkBox_xmode.setChecked(
            settings.value("xmodeOn",
                           QVariant(
                               self.ui.checkBox_xmode.isChecked())).toBool())
        self.ui.checkBox_kill.setChecked(
            settings.value("killOn", QVariant(
                self.ui.checkBox_kill.isChecked())).toBool())
        self.ui.checkBox_startupConnect.setChecked(
            settings.value("startConnect",
                           QVariant(self.ui.checkBox_startupConnect)).toBool())

        self.ui.checkBox_pktHZ.setChecked(
            settings.value("pktHzOn",
                           QVariant(
                               self.ui.checkBox_pktHZ.isChecked())).toBool())
        self.ui.checkBox_logHZ.setChecked(
            settings.value("logHzOn",
                           QVariant(
                               self.ui.checkBox_logHZ.isChecked())).toBool())
        self.ui.horizontalSlider_pktHZ.setValue(
            settings.value(
                "pktHzVal",
                QVariant(self.ui.horizontalSlider_pktHZ.value())).toInt()[0])
        self.ui.horizontalSlider_logHZ.setValue(
            settings.value(
                "logHzVal",
                QVariant(self.ui.horizontalSlider_logHZ.value())).toInt()[0])
        self.ui.horizontalSlider_guiHZ.setValue(
            settings.value(
                "guiHzVal",
                QVariant(self.ui.horizontalSlider_guiHZ.value())).toInt()[0])
        self.ui.horizontalSlider_AI.setValue(
            settings.value(
                "aiHzVal",
                QVariant(self.ui.horizontalSlider_AI.value())).toInt()[0])

        self.logManager.header().restoreState(
            settings.value("logTreeH",
                           self.logManager.header().saveState()).toByteArray())
        self.paramManager.header().restoreState(
            settings.value(
                "paramTreeH",
                self.paramManager.header().saveState()).toByteArray())

    def writeSettings(self):
        """ Write settings to load at next start up """
        rospy.logdebug("Saving session settings")
        settings = QSettings("omwdunkley", "flieROS" + str(self.options.radio))
        settings.setValue("pos", QVariant(self.pos()))
        settings.setValue("size", QVariant(self.size()))

        settings.setValue("autoReconnect",
                          QVariant(self.ui.checkBox_reconnect.isChecked()))
        settings.setValue("beepOn",
                          QVariant(self.ui.checkBox_beep.isChecked()))
        settings.setValue("xmodeOn",
                          QVariant(self.ui.checkBox_xmode.isChecked()))
        settings.setValue("killOn",
                          QVariant(self.ui.checkBox_kill.isChecked()))
        settings.setValue(
            "startConnect",
            QVariant(self.ui.checkBox_startupConnect.isChecked()))

        settings.setValue("pktHzOn",
                          QVariant(self.ui.checkBox_pktHZ.isChecked()))
        settings.setValue("logHzOn",
                          QVariant(self.ui.checkBox_logHZ.isChecked()))
        settings.setValue("pktHzVal",
                          QVariant(self.ui.horizontalSlider_pktHZ.value()))
        settings.setValue("logHzVal",
                          QVariant(self.ui.horizontalSlider_logHZ.value()))
        settings.setValue("guiHzVal",
                          QVariant(self.ui.horizontalSlider_guiHZ.value()))
        settings.setValue("aiHzVal",
                          QVariant(self.ui.horizontalSlider_AI.value()))

        settings.setValue("logTreeH",
                          QVariant(self.logManager.header().saveState()))
        settings.setValue("paramTreeH",
                          QVariant(self.paramManager.header().saveState()))

    @pyqtSlot(bool)
    def setBeep(self, on):
        self.beepOn = on

    @pyqtSlot(bool)
    def setKill(self, on):
        self.killOn = on

    @pyqtSlot(bool)
    def setAutoReconnect(self, on):
        self.autoReconnectOn = on

    @pyqtSlot(bool)
    def setStartupConnect(self, on):
        self.startupConnectOn = on

    def getCRTPStatus(self):
        interface_status = get_interfaces_status()
        self.ui.label_crv.setText(interface_status["radio"])

    def resetInfo(self, initial=False):
        """ Resets the labels on the info tab """
        self.ui.label_baroFound.setText("")
        self.ui.label_magTest.setText("")
        self.ui.label_baroTest.setText("")
        self.ui.label_magFound.setText("")
        self.ui.label_MPUTest.setText("")
        self.ui.label_fw.setText("")
        self.ui.label_fwMod.setText("")
        self.ui.label_crv.setText("")
        self.ui.progressBar_pktIn.setValue(0)
        self.ui.progressBar_pktOut.setValue(0)
        self.ui.progressbar_bat.setValue(3000)
        self.ui.progressbar_link.setValue(0)
        self.ui.progressbar_cpu.setValue(0)
        self.ui.comboBox_connect.setDisabled(False)

        if not initial:
            self.ai.reset()

    @pyqtSlot(int, int)
    def updatePacketRate(self, pb, hz):
        """ Updates the number of pb with hz ensuring we adjust the maximum incase max<hz """
        if hz > pb.maximum():
            rospy.logwarn("Maximum Packets Changed from %d/s -> %d/s",
                          pb.maximum(), hz)
            pb.setMaximum(hz)
        pb.setValue(hz)

    def startScanURI(self):
        """ User Clicked Scan
            Remove all previously found URIs from dropdown
            Disable rescanning
        """
        self.ui.comboBox_connect.clear()

        self.ui.pushButton_connect.setText('Scanning')
        self.ui.pushButton_connect.setDisabled(True)

        #self.scanner.sig_requestScan.emit()
        self.sig_requestScan.emit()

    def receiveScanURI(self, uri):
        """ Results from URI scan
            Add them to dropdown
        """

        # None Found, enable rescanning
        self.ui.pushButton_connect.setDisabled(False)
        if not uri:
            self.ui.pushButton_connect.setText('Scan')
            return

        self.ui.pushButton_connect.setText('Connect')
        for i in uri:
            if len(i[1]) > 0:
                self.ui.comboBox_connect.addItem("%s - %s" % (i[0], i[1]))
            else:
                self.ui.comboBox_connect.addItem(i[0])

        self.ui.comboBox_connect.addItem("Rescan")

        if self.startupConnectOn:
            rospy.loginfo(
                "Auto connecting to first found flie [ConnectOnStartUp = TRUE]"
            )
            self.ui.pushButton_connect.clicked.emit(False)

    def uriSelected(self, uri):
        """ The user clicked on a URI or a scan request
            If SCAN was selected, initiate a scan
        """
        if self.ui.comboBox_connect.currentText() == "Rescan":
            self.startScanURI()

    def connectPressed(self, uri, auto=False):
        """ The user pressed the connect button.
            Either rescan if there is no URI or
            Connect to the flie
        """

        print "------STATE %s ------ MODE %s ------ URI %s" % (
            self.state, "auto" if auto else "manual", uri)

        # No URI Found
        if uri == "":
            self.startScanURI()
            return

        if self.state == STATE.CONNECTED:
            self.requestFlieDisconnect()
        elif self.state == STATE.CONNECTION_RETRYWAIT and not auto:
            # User aborted auto retry
            self.autoRetryTimer.stop()
            self.state = STATE.DISCONNECTED
            self.ui.pushButton_connect.setText("Connect")
            self.ui.comboBox_connect.setEnabled(True)
            print "Aborted auto reconnect"
        else:
            self.requestFlieConnect(uri)

    def requestFlieConnect(self, uri):
        """ Request connection to the flie
        """
        self.ui.pushButton_connect.setText("Connecting...")
        self.ui.pushButton_connect.setEnabled(False)
        self.ui.comboBox_connect.setEnabled(False)
        self.sig_requestConnect.emit(uri)

    def requestFlieDisconnect(self):
        """ Request flie disconnect """

        self.ui.pushButton_connect.setText("Disconnecting...")
        self.sig_requestDisconnect.emit()
        #TODO: UI elements should be changed when the flie reports its disconnected, not when we press the button

    @pyqtSlot(int, str, str)
    def updateFlieState(self, state, uri, msg):
        """ Function that receives all the state updates from the flie.
        """

        if state == STATE.CONNECTION_REQUESTED:
            self.beepMsg(Message(msg="Connection to [%s] requested" % uri))

        elif state == STATE.LINK_ESTABLISHED:
            self.beepMsg(
                Message(msg="Link to [%s] established" % uri,
                        freq=3300,
                        length=25,
                        repeat=1))
            self.ui.pushButton_connect.setText("Download TOC...")
            self.ros.sig_joydataRaw.connect(
                self.flie.sendCmd)  # This needs to be after

        elif state == STATE.CONNECTED:
            self.beepMsg(
                Message(msg="Connected to [%s]" % uri,
                        freq=3500,
                        length=10,
                        repeat=2))
            self.ui.pushButton_connect.setText("Disconnect")
            self.ui.pushButton_connect.setEnabled(True)
            self.getCRTPStatus()
            self.ui.pushButton_genRosMsg.setEnabled(True)

        elif state == STATE.DISCONNECTED:
            self.beepMsg(
                Message(msg="Disconnected from [%s]" % uri, freq=120,
                        length=0))
            self.ui.pushButton_connect.setText("Connect")
            self.ui.pushButton_connect.setEnabled(True)
            self.ros.sig_joydataRaw.disconnect(
                self.flie.sendCmd)  # This needs to be after

        elif state == STATE.CONNECTION_FAILED:
            self.beepMsg(
                Message(msgtype=MSGTYPE.WARN,
                        msg="Connecting to [%s] failed: %s" % (uri, msg)))
            if self.autoReconnectOn:
                rospy.loginfo(
                    "Attempting to auto reconnect after failing to connect")

                #QTimer.singleShot(1000, lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
                self.ui.pushButton_connect.setText("Auto Retrying...")
                self.state = STATE.CONNECTION_RETRYWAIT
                self.ui.pushButton_connect.setEnabled(False)
                self.autoRetryTimer.start()
            else:
                self.ui.pushButton_connect.setText("Connect")
                self.ui.comboBox_connect.setEnabled(True)
                self.ui.pushButton_connect.setEnabled(True)

        elif state == STATE.CONNECTION_LOST:
            self.beepMsg(
                Message(msgtype=MSGTYPE.WARN,
                        msg="Connected lost from [%s]: %s" % (uri, msg),
                        freq=1200,
                        length=10,
                        repeat=6))
            if self.autoReconnectOn:
                rospy.loginfo(
                    "Attempting to auto reconnect after losing connection")
                self.ui.pushButton_connect.setText("Auto Retrying...")
                self.state = STATE.CONNECTION_RETRYWAIT
                self.ui.pushButton_connect.setEnabled(False)
                self.autoRetryTimer.start()
            else:
                self.ui.pushButton_connect.setText("Connect")
                self.ui.comboBox_connect.setEnabled(True)
                self.ui.pushButton_connect.setEnabled(True)

        else:
            rospy.logerr("Unknown State")

        if state > STATE.GEN_DISCONNECTED:
            self.resetInfo()
            self.ui.pushButton_genRosMsg.setEnabled(False)

        self.state = state

    @pyqtSlot(object)  # Message
    def beepMsg(self, msg):
        if self.beepOn and msg.beep:
            os.system("beep -f " + str(msg.f) + "-l " + str(msg.l) + " -r " +
                      str(msg.r) + "&")
        if msg.msg != "":
            if msg.type == MSGTYPE.INFO:
                rospy.loginfo(msg.msg)
            elif msg.type == MSGTYPE.WARN:
                rospy.logwarn(msg.msg)
            elif msg.type == MSGTYPE.ERROR:
                rospy.logerr(msg.msg)
            elif msg.type == MSGTYPE.DEBUG:
                rospy.logdebug(msg.msg)
            else:
                rospy.logerr("UNKNOWN MESSAGE TYPE: %s", msg.msg)
            self.ui.statusbar.showMessage(msg.msg, 0)
Esempio n. 4
0
class DriverWindow(QtGui.QMainWindow ):
    """ Main window and application """

    sig_requestScan = pyqtSignal()
    sig_requestConnect = pyqtSignal(str)
    sig_requestDisconnect = pyqtSignal()

    def __init__(self,options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {"MS5611": self.ui.label_baroTest, "MPU6050": self.ui.label_MPUTest, "HMC5883L": self.ui.label_magTest}

        self.state = STATE.DISCONNECTED


        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)


        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.value() if on else 0 ))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(self.ui.spinBox_pktHZ.value()) # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode( self.ui.checkBox_xmode.isChecked())


        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf, self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)


        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)


        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw*10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw/10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(self.ui.checkBox_yaw.checkState()) # force update
        self.ui.pushButton_north.clicked.connect(lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw()))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(lambda x: self.logManager.setGroundLevel(self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)




        # init previous settings
        self.readSettings()


        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(lambda  x: self.ui.label_up.setText(str(round(x,2))+"ms"))
        self.sync.sig_delayDown.connect(lambda  x: self.ui.label_down.setText(str(round(x,2))+"ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(lambda  x: self.ui.label_cputime.setText("%08.03fs"%x))
        self.sync.sig_flieTime.connect(lambda  x: self.ui.label_flietime.setText("%08.03fs"%x))
        self.sync.sig_diffTime.connect(lambda  x: self.ui.label_difftime.setText("%08.03fs"%x))


        self.ui.checkBox_rosLog.stateChanged.connect(self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(lambda x: self.logManager.setPubToRos(min(self.ui.groupBox_ros.isChecked(), self.ui.checkBox_rosLog.checkState()))) #




        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000/self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(lambda x: self.guiUpdateTimer.setInterval(1000/x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(lambda x: self.ros.sig_joydata.connect(self.setInputJoy) if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)





        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()


        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=False)) # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(self.ui.checkBox_kill.checkState()) # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(self.ui.checkBox_hover.checkState()) # force update



        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(lambda x: self.updateBaroTopics(not x))






        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(lambda found: self.ui.label_baroFound.setText("Yes" if found else "No"))
        self.paramManager.sig_magFound.connect(lambda found: self.ui.label_magFound.setText("Yes" if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect( lambda v : self.setToUpdate("vbat",self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect( lambda v : self.setToUpdate("cpu",self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn,hz))
        self.flie.outKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut,hz))


        # Connections GUI to GUI


        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)


        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()






    @pyqtSlot()
    def updateBaroTopics(self, forceReset=False):
        """ Handles using another baro as a ground station.

        """

        if forceReset:
            if self.ui.pushButton_baro.text() =='Disconnect':
                print 'Disconnecting from', self.ui.comboBox_baro.currentText()
                self.ros.subBaro('')

            self.ui.comboBox_baro.clear()
            self.ui.comboBox_baro.addItem("None Detected")
            self.ui.pushButton_baro.setText('Refresh')

        else:
            if self.ui.pushButton_baro.text() =='Connect':
                # Connect
                self.ui.comboBox_baro.setDisabled(True)
                print 'Connecting to', self.ui.comboBox_baro.currentText()
                self.ros.subBaro(self.ui.comboBox_baro.currentText())
                self.ui.pushButton_baro.setText('Disconnect')


            elif self.ui.pushButton_baro.text() =='Disconnect':
                # Disconnect
                print 'Disconnecting from', self.ui.comboBox_baro.currentText()
                self.ros.subBaro('')
                self.ui.comboBox_baro.clear()
                self.ui.comboBox_baro.addItem("None Detected")
                self.ui.pushButton_baro.setText('Refresh')
                self.ui.comboBox_baro.setDisabled(False)

            else:
                # Scan
                self.ui.comboBox_baro.setDisabled(False)
                self.ui.pushButton_baro.setDisabled(False)
                self.ui.comboBox_baro.clear()
                topics = [t for t in self.ros.getTopics() if t.endswith("/baro") and len(t)>=6 and t[-6]!=str(self.options.radio)]
                if len(topics)>0:
                    self.ui.comboBox_baro.addItems(topics)
                    self.ui.pushButton_baro.setText('Connect')
                else:
                    self.ui.comboBox_baro.addItem("None Detected")




    @pyqtSlot()
    def updateGui(self):
        """ Execute all funcs in the queue """
        #rospy.loginfo("Draw queue size: %d/%s", len(self.guiUpdateQueue), self.guiUpdateQueueSave)
        for f in self.guiUpdateQueue.values():
            f()
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0

    def setToUpdate(self, name, func, *args):
        """ Add functions to call at gui update hz rate"""
        if self.ui.checkBox_guiHZ.isChecked():
            # Limit rate
            self.guiUpdateQueue[name] = partial(func, *args)
            self.guiUpdateQueueSave +=1
        else:
            #dont limit rate
            f = partial(func, *args)
            f()

    @pyqtSlot(float, float, float, float, bool)
    def setInputJoy(self, r, p, y, t, h):
        #TODO: should only do this at a specific hz
        self.setToUpdate("joyR", self.ui.doubleSpinBox_r.setValue, r)
        self.setToUpdate("joyP", self.ui.doubleSpinBox_p.setValue, p)
        self.setToUpdate("joyY", self.ui.doubleSpinBox_y.setValue, y)
        self.setToUpdate("joyT", self.ui.doubleSpinBox_t.setValue, t)
        self.ui.label_hover.setText( "On" if h else "Off")

    @pyqtSlot()
    def genRosMsg(self):
        #TODO: should do this in its own thread, or even as a progress bar
        self.ui.pushButton_genRosMsg.setEnabled(False)
        self.ui.pushButton_genRosMsg.setText("Generating")
        generateRosMessages(self.flie.getLogToC())
        self.ui.pushButton_genRosMsg.setEnabled(True)
        self.ui.pushButton_genRosMsg.setText("Generate ROS msgs from ToC")


    def closeEvent(self, event):
        """ Intercept shutdowns to cleanly disconnect from the flie and cleanly shut ROS down too """
        rospy.loginfo("Close Event")
        #self.autoRetryTimer.stop()

        # Shut Down the Flie
        if self.state < STATE.GEN_DISCONNECTED:
            rospy.loginfo("Triggering flie disconnect for shutdown")
            self.ui.pushButton_connect.setText("Shutting Down...")
            self.flie.requestDisconnect()

        # Clean up ROS
        rospy.signal_shutdown("User closed window")

        # Save State
        self.writeSettings()

        # Commence shutdown
        event.accept()

    def readSettings(self):
        """ Load settings from previous session """
        rospy.logdebug("Loading previous session settings")
        settings = QSettings("omwdunkley", "reset" if self.options.reset else "flieROS"+str(self.options.radio) )
        self.resize(settings.value("size", QVariant(QSize(300, 500))).toSize())
        self.move(settings.value("pos", QVariant(QPoint(200, 200))).toPoint())

        self.ui.checkBox_reconnect.setChecked(settings.value("autoReconnect", QVariant(self.ui.checkBox_reconnect.isChecked())).toBool())
        self.ui.checkBox_beep.setChecked(settings.value("beepOn", QVariant(self.ui.checkBox_beep.isChecked())).toBool())
        self.ui.checkBox_xmode.setChecked(settings.value("xmodeOn", QVariant(self.ui.checkBox_xmode.isChecked())).toBool())
        self.ui.checkBox_kill.setChecked(settings.value("killOn", QVariant(self.ui.checkBox_kill.isChecked())).toBool())
        self.ui.checkBox_startupConnect.setChecked(settings.value("startConnect", QVariant(self.ui.checkBox_startupConnect)).toBool())

        self.ui.checkBox_pktHZ.setChecked(settings.value("pktHzOn", QVariant(self.ui.checkBox_pktHZ.isChecked())).toBool())
        self.ui.checkBox_logHZ.setChecked(settings.value("logHzOn", QVariant(self.ui.checkBox_logHZ.isChecked())).toBool())
        self.ui.horizontalSlider_pktHZ.setValue(settings.value("pktHzVal", QVariant(self.ui.horizontalSlider_pktHZ.value())).toInt()[0])
        self.ui.horizontalSlider_logHZ.setValue(settings.value("logHzVal", QVariant(self.ui.horizontalSlider_logHZ.value())).toInt()[0])
        self.ui.horizontalSlider_guiHZ.setValue(settings.value("guiHzVal", QVariant(self.ui.horizontalSlider_guiHZ.value())).toInt()[0])
        self.ui.horizontalSlider_AI.setValue(settings.value("aiHzVal", QVariant(self.ui.horizontalSlider_AI.value())).toInt()[0])

        self.logManager.header().restoreState(settings.value("logTreeH", self.logManager.header().saveState()).toByteArray())
        self.paramManager.header().restoreState(settings.value("paramTreeH", self.paramManager.header().saveState()).toByteArray())

    def writeSettings(self):
        """ Write settings to load at next start up """
        rospy.logdebug("Saving session settings")
        settings = QSettings("omwdunkley", "flieROS"+str(self.options.radio))
        settings.setValue("pos", QVariant(self.pos()))
        settings.setValue("size", QVariant(self.size()))

        settings.setValue("autoReconnect", QVariant(self.ui.checkBox_reconnect.isChecked()))
        settings.setValue("beepOn", QVariant(self.ui.checkBox_beep.isChecked()))
        settings.setValue("xmodeOn", QVariant(self.ui.checkBox_xmode.isChecked()))
        settings.setValue("killOn", QVariant(self.ui.checkBox_kill.isChecked()))
        settings.setValue("startConnect", QVariant(self.ui.checkBox_startupConnect.isChecked()))

        settings.setValue("pktHzOn", QVariant(self.ui.checkBox_pktHZ.isChecked()))
        settings.setValue("logHzOn", QVariant(self.ui.checkBox_logHZ.isChecked()))
        settings.setValue("pktHzVal", QVariant(self.ui.horizontalSlider_pktHZ.value()))
        settings.setValue("logHzVal", QVariant(self.ui.horizontalSlider_logHZ.value()))
        settings.setValue("guiHzVal", QVariant(self.ui.horizontalSlider_guiHZ.value()))
        settings.setValue("aiHzVal", QVariant(self.ui.horizontalSlider_AI.value()))

        settings.setValue("logTreeH",QVariant(self.logManager.header().saveState()))
        settings.setValue("paramTreeH",QVariant(self.paramManager.header().saveState()))





    @pyqtSlot(bool)
    def setBeep(self, on):
        self.beepOn = on
    @pyqtSlot(bool)
    def setKill(self, on):
        self.killOn = on
    @pyqtSlot(bool)
    def setAutoReconnect(self, on):
        self.autoReconnectOn = on
    @pyqtSlot(bool)
    def setStartupConnect(self, on):
        self.startupConnectOn = on

    def getCRTPStatus(self):
        interface_status =get_interfaces_status()
        self.ui.label_crv.setText(interface_status["radio"])

    def resetInfo(self, initial = False):
        """ Resets the labels on the info tab """
        self.ui.label_baroFound.setText("")
        self.ui.label_magTest.setText("")
        self.ui.label_baroTest.setText("")
        self.ui.label_magFound.setText("")
        self.ui.label_MPUTest.setText("")
        self.ui.label_fw.setText("")
        self.ui.label_fwMod.setText("")
        self.ui.label_crv.setText("")
        self.ui.progressBar_pktIn.setValue(0)
        self.ui.progressBar_pktOut.setValue(0)
        self.ui.progressbar_bat.setValue(3000)
        self.ui.progressbar_link.setValue(0)
        self.ui.progressbar_cpu.setValue(0)
        self.ui.comboBox_connect.setDisabled(False)

        if not initial:
            self.ai.reset()

    @pyqtSlot(int,int)
    def updatePacketRate(self, pb, hz):
        """ Updates the number of pb with hz ensuring we adjust the maximum incase max<hz """
        if hz>pb.maximum():
            rospy.logwarn("Maximum Packets Changed from %d/s -> %d/s", pb.maximum(), hz)
            pb.setMaximum(hz)
        pb.setValue(hz)


    def startScanURI(self):
        """ User Clicked Scan
            Remove all previously found URIs from dropdown
            Disable rescanning
        """
        self.ui.comboBox_connect.clear()

        self.ui.pushButton_connect.setText('Scanning')
        self.ui.pushButton_connect.setDisabled(True)

        #self.scanner.sig_requestScan.emit()
        self.sig_requestScan.emit()




    def receiveScanURI(self, uri):
        """ Results from URI scan
            Add them to dropdown
        """

        # None Found, enable rescanning
        self.ui.pushButton_connect.setDisabled(False)
        if not uri:
            self.ui.pushButton_connect.setText('Scan')
            return

        self.ui.pushButton_connect.setText('Connect')
        for i in uri:
            if len(i[1]) > 0:
                self.ui.comboBox_connect.addItem("%s - %s" % (i[0], i[1]))
            else:
                self.ui.comboBox_connect.addItem(i[0])

        self.ui.comboBox_connect.addItem("Rescan")

        if self.startupConnectOn:
            rospy.loginfo("Auto connecting to first found flie [ConnectOnStartUp = TRUE]")
            self.ui.pushButton_connect.clicked.emit(False)


    def uriSelected(self, uri):
        """ The user clicked on a URI or a scan request
            If SCAN was selected, initiate a scan
        """
        if self.ui.comboBox_connect.currentText() == "Rescan":
            self.startScanURI()

    def connectPressed(self, uri, auto=False):
        """ The user pressed the connect button.
            Either rescan if there is no URI or
            Connect to the flie
        """


        print "------STATE %s ------ MODE %s ------ URI %s" % (self.state, "auto" if auto else "manual", uri)


        # No URI Found
        if uri=="":
            self.startScanURI()
            return


        if self.state == STATE.CONNECTED:
            self.requestFlieDisconnect()
        elif self.state == STATE.CONNECTION_RETRYWAIT and not auto:
            # User aborted auto retry
            self.autoRetryTimer.stop()
            self.state = STATE.DISCONNECTED
            self.ui.pushButton_connect.setText("Connect")
            self.ui.comboBox_connect.setEnabled(True)
            print "Aborted auto reconnect"
        else:
            self.requestFlieConnect(uri)

    def requestFlieConnect(self, uri):
        """ Request connection to the flie
        """
        self.ui.pushButton_connect.setText("Connecting...")
        self.ui.pushButton_connect.setEnabled(False)
        self.ui.comboBox_connect.setEnabled(False)
        self.sig_requestConnect.emit(uri)


    def requestFlieDisconnect(self):
        """ Request flie disconnect """

        self.ui.pushButton_connect.setText("Disconnecting...")
        self.sig_requestDisconnect.emit()
        #TODO: UI elements should be changed when the flie reports its disconnected, not when we press the button



    @pyqtSlot(int, str, str)
    def updateFlieState(self, state, uri, msg):
        """ Function that receives all the state updates from the flie.
        """

        if state == STATE.CONNECTION_REQUESTED:
            self.beepMsg(Message(msg="Connection to [%s] requested" % uri))

        elif state == STATE.LINK_ESTABLISHED:
            self.beepMsg(Message(msg="Link to [%s] established" % uri, freq=3300, length=25, repeat=1))
            self.ui.pushButton_connect.setText("Download TOC...")
            self.ros.sig_joydataRaw.connect(self.flie.sendCmd) # This needs to be after

        elif state == STATE.CONNECTED:
            self.beepMsg(Message(msg="Connected to [%s]" % uri, freq=3500, length=10, repeat=2))
            self.ui.pushButton_connect.setText("Disconnect")
            self.ui.pushButton_connect.setEnabled(True)
            self.getCRTPStatus()
            self.ui.pushButton_genRosMsg.setEnabled(True)

        elif state == STATE.DISCONNECTED:
            self.beepMsg(Message(msg="Disconnected from [%s]" % uri, freq=120, length=0))
            self.ui.pushButton_connect.setText("Connect")
            self.ui.pushButton_connect.setEnabled(True)
            self.ros.sig_joydataRaw.disconnect(self.flie.sendCmd) # This needs to be after

        elif state == STATE.CONNECTION_FAILED:
            self.beepMsg(Message(msgtype=MSGTYPE.WARN, msg="Connecting to [%s] failed: %s" % (uri, msg)))
            if self.autoReconnectOn:
                rospy.loginfo("Attempting to auto reconnect after failing to connect")

                #QTimer.singleShot(1000, lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
                self.ui.pushButton_connect.setText("Auto Retrying...")
                self.state = STATE.CONNECTION_RETRYWAIT
                self.ui.pushButton_connect.setEnabled(False)
                self.autoRetryTimer.start()
            else:
                self.ui.pushButton_connect.setText("Connect")
                self.ui.comboBox_connect.setEnabled(True)
                self.ui.pushButton_connect.setEnabled(True)

        elif state == STATE.CONNECTION_LOST:
            self.beepMsg(Message(msgtype=MSGTYPE.WARN, msg="Connected lost from [%s]: %s" % (uri, msg), freq=1200, length=10, repeat=6))
            if self.autoReconnectOn:
                rospy.loginfo("Attempting to auto reconnect after losing connection")
                self.ui.pushButton_connect.setText("Auto Retrying...")
                self.state = STATE.CONNECTION_RETRYWAIT
                self.ui.pushButton_connect.setEnabled(False)
                self.autoRetryTimer.start()
            else:
                self.ui.pushButton_connect.setText("Connect")
                self.ui.comboBox_connect.setEnabled(True)
                self.ui.pushButton_connect.setEnabled(True)

        else:
            rospy.logerr("Unknown State")

        if state>STATE.GEN_DISCONNECTED:
            self.resetInfo()
            self.ui.pushButton_genRosMsg.setEnabled(False)

        self.state = state


    @pyqtSlot(object) # Message
    def beepMsg(self, msg):
        if self.beepOn and msg.beep:
            os.system("beep -f "+str(msg.f)+"-l "+str(msg.l)+" -r "+str(msg.r)+"&")
        if msg.msg != "":
            if msg.type == MSGTYPE.INFO:
                rospy.loginfo(msg.msg)
            elif msg.type == MSGTYPE.WARN:
                rospy.logwarn(msg.msg)
            elif msg.type == MSGTYPE.ERROR:
                rospy.logerr(msg.msg)
            elif msg.type == MSGTYPE.DEBUG:
                rospy.logdebug(msg.msg)
            else:
                rospy.logerr("UNKNOWN MESSAGE TYPE: %s", msg.msg)
            self.ui.statusbar.showMessage(msg.msg, 0)