class FlightTab(Tab, flight_tab_class):
    uiSetupReadySignal = pyqtSignal()

    _motor_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)
    _assisted_control_updated_signal = pyqtSignal(bool)
    _heighthold_input_updated_signal = pyqtSignal(float, float, float, float)
    _hover_input_updated_signal = pyqtSignal(float, float, float, float)

    _log_error_signal = pyqtSignal(object, str)

    # UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    _limiting_updated = pyqtSignal(bool, bool, bool)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
            self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
            self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
            self._emergency_stop_updated_signal.emit)

        self.helper.inputDeviceReader.heighthold_input_updated.add_callback(
            self._heighthold_input_updated_signal.emit)
        self._heighthold_input_updated_signal.connect(
            self._heighthold_input_updated)
        self.helper.inputDeviceReader.hover_input_updated.add_callback(
            self._hover_input_updated_signal.emit)
        self._hover_input_updated_signal.connect(self._hover_input_updated)

        self.helper.inputDeviceReader.assisted_control_updated.add_callback(
            self._assisted_control_updated_signal.emit)

        self._assisted_control_updated_signal.connect(
            self._assisted_control_updated)

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._motor_data_signal.connect(self._motor_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))

        self.crazyflieXModeCheckbox.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.x", str(enabled)))
        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="xmode",
            cb=(lambda name, checked: self.crazyflieXModeCheckbox.setChecked(
                eval(checked))))

        self.ratePidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(enabled)))

        self.angularPidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(not enabled)))

        self._led_ring_headlight.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "ring.headlightEnable", str(enabled)))

        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="ratepid",
            cb=(lambda name, checked: self.ratePidRadioButton.setChecked(
                eval(checked))))

        self.helper.cf.param.add_update_callback(
            group="cpu", name="flash", cb=self._set_enable_client_xmode)

        self.helper.cf.param.add_update_callback(
            group="ring",
            name="headlightEnable",
            cb=(lambda name, checked: self._led_ring_headlight.setChecked(
                eval(checked))))

        self._ledring_nbr_effects = 0

        self.helper.cf.param.add_update_callback(group="ring",
                                                 name="effect",
                                                 cb=self._ring_effect_updated)

        self.helper.cf.param.add_update_callback(
            group="imu_sensors", cb=self._set_available_sensors)

        self.helper.cf.param.all_updated.add_callback(
            self._ring_populate_dropdown)

        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000, 1])

        self.targetCalPitch.setValue(Config().get("trim_pitch"))
        self.targetCalRoll.setValue(Config().get("trim_roll"))

        self.helper.inputDeviceReader.alt1_updated.add_callback(
            self.alt1_updated)
        self.helper.inputDeviceReader.alt2_updated.add_callback(
            self.alt2_updated)
        self._tf_state = 0
        self._ring_effect = 0

        # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
        self.helper.inputDeviceReader.limiting_updated.add_callback(
            self._limiting_updated.emit)
        self._limiting_updated.connect(self._set_limiting_enabled)

    def _set_enable_client_xmode(self, name, value):
        if eval(value) <= 128:
            self.clientXModeCheckbox.setEnabled(True)
        else:
            self.clientXModeCheckbox.setEnabled(False)
            self.clientXModeCheckbox.setChecked(False)

    def _set_limiting_enabled(self, rp_limiting_enabled, yaw_limiting_enabled,
                              thrust_limiting_enabled):
        self.maxAngle.setEnabled(rp_limiting_enabled)
        self.targetCalRoll.setEnabled(rp_limiting_enabled)
        self.targetCalPitch.setEnabled(rp_limiting_enabled)
        self.maxYawRate.setEnabled(yaw_limiting_enabled)
        self.maxThrust.setEnabled(thrust_limiting_enabled)
        self.minThrust.setEnabled(thrust_limiting_enabled)
        self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
        self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
            Config().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(
            self, "Log error",
            "Error when starting log config [%s]: %s" % (log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])

    def _baro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            estimated_z = data[LOG_NAME_ESTIMATED_Z]
            self.actualHeight.setText(("%.2f" % estimated_z))
            self.ai.setBaro(estimated_z, self.is_visible())

    def _heighthold_input_updated(self, roll, pitch, yaw, height):
        if (self.isVisible() and
            (self.helper.inputDeviceReader.get_assisted_control()
             == self.helper.inputDeviceReader.ASSISTED_CONTROL_HEIGHTHOLD)):
            self.targetRoll.setText(("%0.2f deg" % roll))
            self.targetPitch.setText(("%0.2f deg" % pitch))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

    def _hover_input_updated(self, vx, vy, yaw, height):
        if (self.isVisible()
                and (self.helper.inputDeviceReader.get_assisted_control()
                     == self.helper.inputDeviceReader.ASSISTED_CONTROL_HOVER)):
            self.targetRoll.setText(("%0.2f m/s" % vy))
            self.targetPitch.setText(("%0.2f m/s" % vx))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

    def _imu_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText(
                "%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))

            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"], self.is_visible())

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        # MOTOR
        lg = LogConfig("Motors", Config().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        self._populate_assisted_mode_dropdown()

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)

        self.actualHeight.setEnabled(True)
        self.helper.inputDeviceReader.set_alt_hold_available(available)
        if not self.logBaro:
            # The sensor is available, set up the logging
            self.logBaro = LogConfig("Baro", 200)
            self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")

            try:
                self.helper.cf.log.add_config(self.logBaro)
                self.logBaro.data_received_cb.add_callback(
                    self._baro_data_signal.emit)
                self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
                self.logBaro.start()
            except KeyError as e:
                logger.warning(str(e))
            except AttributeError as e:
                logger.warning(str(e))

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualHeight.setText("")
        self.targetHeight.setText("Not Set")
        self.ai.setHover(0, self.is_visible())
        self.targetHeight.setEnabled(False)
        self.actualHeight.setEnabled(False)
        self.clientXModeCheckbox.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None
        self._led_ring_effect.setEnabled(False)
        self._led_ring_effect.clear()
        try:
            self._led_ring_effect.currentIndexChanged.disconnect(
                self._ring_effect_changed)
        except TypeError:
            # Signal was not connected
            pass
        self._led_ring_effect.setCurrentIndex(-1)
        self._led_ring_headlight.setEnabled(False)

        try:
            self._assist_mode_combo.currentIndexChanged.disconnect(
                self._assist_mode_changed)
        except TypeError:
            # Signal was not connected
            pass
        self._assist_mode_combo.setEnabled(False)
        self._assist_mode_combo.clear()

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
        self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("min_thrust", self.minThrust.value())
            Config().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.thrust_slew_rate = (
            self.thrustLoweringSlewRateLimit.value())
        self.helper.inputDeviceReader.thrust_slew_limit = (
            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode is True):
            Config().set("slew_limit", self.slewEnableLimit.value())
            Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_pitch = value
        Config().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_roll = value
        Config().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f deg" % roll))
        self.targetPitch.setText(("%0.2f deg" % pitch))
        self.targetYaw.setText(("%0.2f deg/s" % yaw))
        self.targetThrust.setText(
            ("%0.2f %%" % self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
        logger.debug("Changed flightmode to %s",
                     self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(Config().get("normal_max_rp"))
            self.maxThrust.setValue(Config().get("normal_max_thrust"))
            self.minThrust.setValue(Config().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("normal_slew_rate"))
            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(Config().get("max_rp"))
            self.maxThrust.setValue(Config().get("max_thrust"))
            self.minThrust.setValue(Config().get("min_thrust"))
            self.slewEnableLimit.setValue(Config().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("slew_rate"))
            self.maxYawRate.setValue(Config().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    def _assist_mode_changed(self, item):
        mode = None

        if (item == 0):  # Altitude hold
            mode = JoystickReader.ASSISTED_CONTROL_ALTHOLD
        if (item == 1):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_POSHOLD
        if (item == 2):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD
        if (item == 3):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HOVER

        self.helper.inputDeviceReader.set_assisted_control(mode)
        Config().set("assistedControl", mode)

    def _assisted_control_updated(self, enabled):
        if self.helper.inputDeviceReader.get_assisted_control() == \
                JoystickReader.ASSISTED_CONTROL_POSHOLD:
            self.targetThrust.setEnabled(not enabled)
            self.targetRoll.setEnabled(not enabled)
            self.targetPitch.setEnabled(not enabled)
        elif ((self.helper.inputDeviceReader.get_assisted_control()
               == JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD)
              or (self.helper.inputDeviceReader.get_assisted_control()
                  == JoystickReader.ASSISTED_CONTROL_HOVER)):
            self.targetThrust.setEnabled(not enabled)
            self.targetHeight.setEnabled(enabled)
        else:
            self.helper.cf.param.set_value("flightmode.althold", str(enabled))

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        Config().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)

    def alt1_updated(self, state):
        if state:
            new_index = (self._ring_effect + 1) % (self._ledring_nbr_effects +
                                                   1)
            self.helper.cf.param.set_value("ring.effect", str(new_index))

    def alt2_updated(self, state):
        self.helper.cf.param.set_value("ring.headlightEnable", str(state))

    def _ring_populate_dropdown(self):
        try:
            nbr = int(self.helper.cf.param.values["ring"]["neffect"])
            current = int(self.helper.cf.param.values["ring"]["effect"])
        except KeyError:
            return

        # Used only in alt1_updated function
        self._ring_effect = current
        self._ledring_nbr_effects = nbr

        hardcoded_names = {
            0: "Off",
            1: "White spinner",
            2: "Color spinner",
            3: "Tilt effect",
            4: "Brightness effect",
            5: "Color spinner 2",
            6: "Double spinner",
            7: "Solid color effect",
            8: "Factory test",
            9: "Battery status",
            10: "Boat lights",
            11: "Alert",
            12: "Gravity",
            13: "LED tab",
            14: "Color fader",
            15: "Link quality"
        }

        for i in range(nbr + 1):
            name = "{}: ".format(i)
            if i in hardcoded_names:
                name += hardcoded_names[i]
            else:
                name += "N/A"
            self._led_ring_effect.addItem(name, i)

        self._led_ring_effect.currentIndexChanged.connect(
            self._ring_effect_changed)

        self._led_ring_effect.setCurrentIndex(current)
        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
            self._led_ring_effect.setEnabled(True)
            self._led_ring_headlight.setEnabled(True)

    def _ring_effect_changed(self, index):
        self._ring_effect = index
        if index > -1:
            i = self._led_ring_effect.itemData(index)
            logger.info("Changed effect to {}".format(i))
            if i != int(self.helper.cf.param.values["ring"]["effect"]):
                self.helper.cf.param.set_value("ring.effect", str(i))

    def _ring_effect_updated(self, name, value):
        if self.helper.cf.param.is_updated:
            self._led_ring_effect.setCurrentIndex(int(value))

    def _populate_assisted_mode_dropdown(self):
        self._assist_mode_combo.addItem("Altitude hold", 0)
        self._assist_mode_combo.addItem("Position hold", 1)
        self._assist_mode_combo.addItem("Height hold", 2)
        self._assist_mode_combo.addItem("Hover", 3)
        heightHoldPossible = False
        hoverPossible = False

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x09):
            heightHoldPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(1.0)

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0E):
            heightHoldPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(2.0)

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0A):
            heightHoldPossible = True
            hoverPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(1.0)

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0F):
            heightHoldPossible = True
            hoverPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(2.0)

        if not heightHoldPossible:
            self._assist_mode_combo.model().item(2).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        if not hoverPossible:
            self._assist_mode_combo.model().item(3).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        self._assist_mode_combo.currentIndexChanged.connect(
            self._assist_mode_changed)
        self._assist_mode_combo.setEnabled(True)

        try:
            assistmodeComboIndex = Config().get("assistedControl")
            if assistmodeComboIndex == 3 and not hoverPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and hoverPossible:
                self._assist_mode_combo.setCurrentIndex(3)
                self._assist_mode_combo.currentIndexChanged.emit(3)
            elif assistmodeComboIndex == 2 and not heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(2)
                self._assist_mode_combo.currentIndexChanged.emit(2)
            else:
                self._assist_mode_combo.setCurrentIndex(assistmodeComboIndex)
                self._assist_mode_combo.currentIndexChanged.emit(
                    assistmodeComboIndex)
        except KeyError:
            defaultOption = 0
            if hoverPossible:
                defaultOption = 3
            elif heightHoldPossible:
                defaultOption = 2
            self._assist_mode_combo.setCurrentIndex(defaultOption)
            self._assist_mode_combo.currentIndexChanged.emit(defaultOption)
예제 #2
0
class SwarmTab(Tab, example_tab_class):
    uiSetupReadySignal = pyqtSignal()

    _motor_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)
    _assisted_control_updated_signal = pyqtSignal(bool)
    _heighthold_input_updated_signal = pyqtSignal(float, float, float, float)
    _hover_input_updated_signal = pyqtSignal(float, float, float, float)

    _log_error_signal = pyqtSignal(object, str)

    _plotter_log_error_signal = pyqtSignal(object, str)
    _log_data_signal = pyqtSignal(int, object, object)
    _disconnected_signal = pyqtSignal(str)
    _connected_signal = pyqtSignal(str)

    colors = [
        (60, 200, 60),    # green
        (40, 100, 255),   # blue
        (255, 130, 240),  # magenta
        (255, 26, 28),    # red
        (255, 170, 0),    # orange
        (40, 180, 240),   # cyan
        (153, 153, 153),  # grey
        (176, 96, 50),    # brown
        (180, 60, 240),   # purple
    ]

    # UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    _limiting_updated = pyqtSignal(bool, bool, bool)

    def __init__(self, tabWidget, helper, *args):
        super(SwarmTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Swarm"
        self.menuName = "Swarm"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incoming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
            self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
            self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
            self._emergency_stop_updated_signal.emit)

        self.helper.inputDeviceReader.heighthold_input_updated.add_callback(
            self._heighthold_input_updated_signal.emit)
        self._heighthold_input_updated_signal.connect(
            self._heighthold_input_updated)
        self.helper.inputDeviceReader.hover_input_updated.add_callback(
            self._hover_input_updated_signal.emit)
        self._hover_input_updated_signal.connect(
            self._hover_input_updated)

        self.helper.inputDeviceReader.assisted_control_updated.add_callback(
            self._assisted_control_updated_signal.emit)

        self._assisted_control_updated_signal.connect(
            self._assisted_control_updated)

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._motor_data_signal.connect(self._motor_data_received)

        self._log_error_signal.connect(self._logging_error)

        # PlotWidget Stuff
        self._plot = PlotWidget(fps=30)
        # Check if we could find the PyQtImport. If not, then
        # set this tab as disabled
        self.enabled = self._plot.can_enable
        self._model = LogConfigModel()
        self.dataSelector.setModel(self._model)
        self.plotLayout.addWidget(self._plot)
        self._log_data_signal.connect(self._log_data_received)
        self._plotter_log_error_signal.connect(self._plotter_logging_error)

        if self.enabled:
            self._disconnected_signal.connect(self._disconnected)
            self.helper.cf.disconnected.add_callback(
                self._disconnected_signal.emit)

            self._connected_signal.connect(self._connected)
            self.helper.cf.connected.add_callback(
                self._connected_signal.emit)

            self.helper.cf.log.block_added_cb.add_callback(self._config_added)
            self.dataSelector.currentIndexChanged.connect(
                self._selection_changed)

        self._previous_config = None
        self._started_previous = False

        # Connect UI signals that are in this tab
        #self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        # self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        # self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        # self.thrustLoweringSlewRateLimit.valueChanged.connect(
        #     self.thrustLoweringSlewRateLimitChanged)
        # self.slewEnableLimit.valueChanged.connect(
        #     self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.inputTypeComboBox.currentIndexChanged.connect(self.inputtypeChange)
        self.stepHeightCombo.valueChanged.connect(self._step_height_changed)
        self.frequencyCombo.valueChanged.connect(self._sine_frequency_changed)
        self.referenceHeightCheckbox.toggled.connect(lambda: self._use_ref_input(self.referenceHeightCheckbox.isChecked()))
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.updateGainsBtn.clicked.connect(self._controller_gain_changed)
        self.k1Combo.valueChanged.connect(self._k123_gain_changed)
        self.k2Combo.valueChanged.connect(self._k123_gain_changed)
        self.k3Combo.valueChanged.connect(self._k123_gain_changed)
        # self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        # self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        # self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))
        #
        # self.crazyflieXModeCheckbox.clicked.connect(
        #     lambda enabled:
        #     self.helper.cf.param.set_value("flightmode.x",
        #                                    str(enabled)))
        # self.helper.cf.param.add_update_callback(
        #     group="flightmode", name="xmode",
        #     cb=(lambda name, checked:
        #         self.crazyflieXModeCheckbox.setChecked(eval(checked))))
        #
        # self.ratePidRadioButton.clicked.connect(
        #     lambda enabled:
        #     self.helper.cf.param.set_value("flightmode.ratepid",
        #                                    str(enabled)))
        #
        # self.angularPidRadioButton.clicked.connect(
        #     lambda enabled:
        #     self.helper.cf.param.set_value("flightmode.ratepid",
        #                                    str(not enabled)))
        #
        # self._led_ring_headlight.clicked.connect(
        #     lambda enabled:
        #     self.helper.cf.param.set_value("ring.headlightEnable",
        #                                    str(enabled)))

        # self.helper.cf.param.add_update_callback(
        #     group="flightmode", name="ratepid",
        #     cb=(lambda name, checked:
        #         self.ratePidRadioButton.setChecked(eval(checked))))

        # self.helper.cf.param.add_update_callback(
        #     group="cpu", name="flash",
        #     cb=self._set_enable_client_xmode)

        # self.helper.cf.param.add_update_callback(
        #     group="ring", name="headlightEnable",
        #     cb=(lambda name, checked:
        #         self._led_ring_headlight.setChecked(eval(checked))))

        self._ledring_nbr_effects = 0

        # self.helper.cf.param.add_update_callback(
        #     group="ring",
        #     name="effect",
        #     cb=self._ring_effect_updated)

        self.helper.cf.param.add_update_callback(
            group="imu_sensors",
            cb=self._set_available_sensors)

        # self.helper.cf.param.all_updated.add_callback(
        #     self._ring_populate_dropdown)

        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_2.addWidget(self.ai)
        # self.logo = QLabel(self)
        # pixmap = QPixmap('C:\dev\crazyflie-clients-python\src\cfclient/SUTDLogo.png')
        # self.logo.setPixmap(pixmap.scaled(150, 150, Qt.KeepAspectRatio))
        # self.verticalLayout.addWidget(self.logo)
        self.splitter.setSizes([1000, 1])

        self.targetCalPitch.setValue(Config().get("trim_pitch"))
        self.targetCalRoll.setValue(Config().get("trim_roll"))

        self.helper.inputDeviceReader.alt1_updated.add_callback(
            self.alt1_updated)
        self.helper.inputDeviceReader.alt2_updated.add_callback(
            self.alt2_updated)
        self._tf_state = 0
        self._ring_effect = 0

        # # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
        # self.helper.inputDeviceReader.limiting_updated.add_callback(
        #     self._limiting_updated.emit)
        # self._limiting_updated.connect(self._set_limiting_enabled)

    def _set_enable_client_xmode(self, name, value):
        if eval(value) <= 128:
            self.clientXModeCheckbox.setEnabled(True)
        else:
            self.clientXModeCheckbox.setEnabled(False)
            self.clientXModeCheckbox.setChecked(False)

    def _set_limiting_enabled(self, rp_limiting_enabled,
                              yaw_limiting_enabled,
                              thrust_limiting_enabled):
        self.maxAngle.setEnabled(rp_limiting_enabled)
        self.targetCalRoll.setEnabled(rp_limiting_enabled)
        self.targetCalPitch.setEnabled(rp_limiting_enabled)
        self.maxYawRate.setEnabled(yaw_limiting_enabled)
        self.maxThrust.setEnabled(thrust_limiting_enabled)
        self.minThrust.setEnabled(thrust_limiting_enabled)
        self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
        self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
            Config().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error",
                          "Error when starting log config [%s]: %s" % (
                              log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])

    def _baro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            estimated_z = data[LOG_NAME_ESTIMATED_Z]
            self.actualHeight.setText(("%.2f" % estimated_z))
            self.ai.setBaro(estimated_z, self.is_visible())

    def _heighthold_input_updated(self, roll, pitch, yaw, height):
        if (self.isVisible() and
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 self.helper.inputDeviceReader.ASSISTED_CONTROL_HEIGHTHOLD)):
            self.targetRoll.setText(("%0.2f deg" % roll))
            self.targetPitch.setText(("%0.2f deg" % pitch))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

    def _hover_input_updated(self, vx, vy, yaw, height):
        if (self.isVisible() and
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 self.helper.inputDeviceReader.ASSISTED_CONTROL_HOVER)):
            self.targetRoll.setText(("%0.2f m/s" % vy))
            self.targetPitch.setText(("%0.2f m/s" % vx))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

    def _imu_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText("%.2f%%" %
                                      self.thrustToPercentage(
                                          data["stabilizer.thrust"]))

            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"], self.is_visible())

    def connected(self, linkURI):
        # Reset Gains
        self.helper.cf.param.set_value("posCtlPid.zKd", str(0.0))
        self.helper.cf.param.set_value("posCtlPid.zKi", str(0.5))
        self.helper.cf.param.set_value("posCtlPid.zKp", str(40.0))
        self.k1Combo.setValue(40.0)
        self.helper.cf.param.set_value("posCtlPid.zVelMax", str(15.0))
        self.helper.cf.param.set_value("velCtlPid.vzKd", str(0.0))
        self.helper.cf.param.set_value("velCtlPid.vzKi", str(0.0))
        self.helper.cf.param.set_value("velCtlPid.vzKp", str(15.0))
        self.k2Combo.setValue(15.0)
        self.k3Combo.setValue(0.0)

        # IMU & THRUST
        lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        # MOTOR
        lg = LogConfig("Motors", Config().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        self._populate_assisted_mode_dropdown()

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)

        self.actualHeight.setEnabled(True)
        self.helper.inputDeviceReader.set_alt_hold_available(available)
        if not self.logBaro:
            # The sensor is available, set up the logging
            self.logBaro = LogConfig("Baro", 200)
            self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")

            try:
                self.helper.cf.log.add_config(self.logBaro)
                self.logBaro.data_received_cb.add_callback(
                    self._baro_data_signal.emit)
                self.logBaro.error_cb.add_callback(
                    self._log_error_signal.emit)
                self.logBaro.start()
            except KeyError as e:
                logger.warning(str(e))
            except AttributeError as e:
                logger.warning(str(e))

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualHeight.setText("")
        self.targetHeight.setText("Not Set")
        self.ai.setHover(0, self.is_visible())
        self.targetHeight.setEnabled(False)
        self.actualHeight.setEnabled(False)
        self.clientXModeCheckbox.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None
        # self._led_ring_effect.setEnabled(False)
        # self._led_ring_effect.clear()
        # try:
        #     self._led_ring_effect.currentIndexChanged.disconnect(
        #         self._ring_effect_changed)
        # except TypeError:
        #     # Signal was not connected
        #     pass
        # self._led_ring_effect.setCurrentIndex(-1)
        # self._led_ring_headlight.setEnabled(False)

        try:
            self._assist_mode_combo.currentIndexChanged.disconnect(
                self._assist_mode_changed)
        except TypeError:
            # Signal was not connected
            pass
        self._assist_mode_combo.setEnabled(False)
        self._assist_mode_combo.clear()

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
        self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("min_thrust", self.minThrust.value())
            Config().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.thrust_slew_rate = (
            self.thrustLoweringSlewRateLimit.value())
        self.helper.inputDeviceReader.thrust_slew_limit = (
            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode is True):
            Config().set("slew_limit", self.slewEnableLimit.value())
            Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_pitch = value
        Config().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_roll = value
        Config().set("trim_roll", value)

    def _k123_gain_changed(self):
        logger.debug("controller gains updated")
        self.updateGainsBtn.setStyleSheet("background-color:#0078d7; color:#ffffff;")
        self.resetGainsBtn.setStyleSheet("background-color:#0078d7; color:#ffffff;")

    def _controller_gain_changed(self):
        self.updateGainsBtn.setStyleSheet("background-color:#f0f0f0; color:#000000;")
        self.helper.cf.param.set_value("posCtlPid.zKi", str(self.k3Combo.value()))
        self.helper.cf.param.set_value("posCtlPid.zKp", str(self.k1Combo.value()))
        self.helper.cf.param.set_value("velCtlPid.vzKp", str(self.k2Combo.value()))

    def _reset_gain_changed(self):
        # Reset Gains
        self.k1Combo.value(40.0)
        self.k2Combo.setValue(15.0)
        self.k3Combo.setValue(0.0)
        self.helper.cf.param.set_value("posCtlPid.zKi", str(self.k3Combo.value()))
        self.helper.cf.param.set_value("posCtlPid.zKp", str(self.k1Combo.value()))
        self.helper.cf.param.set_value("velCtlPid.vzKp", str(self.k2Combo.value()))
        self.resetGainsBtn.setStyleSheet("background-color:#f0f0f0; color:#000000;")

    def _step_height_changed(self, reference):
        logger.debug("Reference height updated to [%f]" % reference)
        self.targetHeight.setText(("%.2f m" % reference))
        self.ai.setHover(reference, self.is_visible())
        self.helper.referenceHeight = reference
        self.helper.inputTime = 0.0

    def _sine_frequency_changed(self, freq):
        logger.debug("Sine-wave frequency updated to [%f]" % freq)
        self.helper.sinewaveFrequency = freq
        self.helper.inputTime = 0.0

    def _use_ref_input(self, enabled):
        logger.debug("Using reference input")
        self.helper.useReferenceHeight = enabled
        self.helper.inputTime = 0.0

    def inputtypeChange(self, item):
        logger.debug("Changed input type to %s",
                     self.inputTypeComboBox.itemText(item))
        self.helper.inputType = item
        self.helper.inputTime = 0.0
        # item is a number, #0 = 'Step Input', #1 = 'Sine Wave', #2 = 'Ramp'

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f deg" % roll))
        self.targetPitch.setText(("%0.2f deg" % pitch))
        self.targetYaw.setText(("%0.2f deg/s" % yaw))
        self.targetThrust.setText(("%0.2f %%" %
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
        logger.debug("Changed flightmode to %s",
                     self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(Config().get("normal_max_rp"))
            self.maxThrust.setValue(Config().get("normal_max_thrust"))
            self.minThrust.setValue(Config().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("normal_slew_rate"))
            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(Config().get("max_rp"))
            self.maxThrust.setValue(Config().get("max_thrust"))
            self.minThrust.setValue(Config().get("min_thrust"))
            self.slewEnableLimit.setValue(Config().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("slew_rate"))
            self.maxYawRate.setValue(Config().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    def _assist_mode_changed(self, item):
        mode = None

        if (item == 0):  # Altitude hold
            mode = JoystickReader.ASSISTED_CONTROL_ALTHOLD
        if (item == 1):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_POSHOLD
        if (item == 2):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD
        if (item == 3):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HOVER

        self.helper.inputDeviceReader.set_assisted_control(mode)
        Config().set("assistedControl", mode)

    def _assisted_control_updated(self, enabled):
        if self.helper.inputDeviceReader.get_assisted_control() == \
                JoystickReader.ASSISTED_CONTROL_POSHOLD:
            self.targetThrust.setEnabled(not enabled)
            self.targetRoll.setEnabled(not enabled)
            self.targetPitch.setEnabled(not enabled)
        elif ((self.helper.inputDeviceReader.get_assisted_control() ==
                JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD) or
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 JoystickReader.ASSISTED_CONTROL_HOVER)):
            self.targetThrust.setEnabled(not enabled)
            self.targetHeight.setEnabled(enabled)
        else:
            self.helper.cf.param.set_value("flightmode.althold", str(enabled))

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        Config().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)

    def alt1_updated(self, state):
        if state:
            new_index = (self._ring_effect+1) % (self._ledring_nbr_effects+1)
            self.helper.cf.param.set_value("ring.effect",
                                           str(new_index))

    def alt2_updated(self, state):
        self.helper.cf.param.set_value("ring.headlightEnable", str(state))

    def _ring_populate_dropdown(self):
        try:
            nbr = int(self.helper.cf.param.values["ring"]["neffect"])
            current = int(self.helper.cf.param.values["ring"]["effect"])
        except KeyError:
            return

        # Used only in alt1_updated function
        self._ring_effect = current
        self._ledring_nbr_effects = nbr

        hardcoded_names = {0: "Off",
                           1: "White spinner",
                           2: "Color spinner",
                           3: "Tilt effect",
                           4: "Brightness effect",
                           5: "Color spinner 2",
                           6: "Double spinner",
                           7: "Solid color effect",
                           8: "Factory test",
                           9: "Battery status",
                           10: "Boat lights",
                           11: "Alert",
                           12: "Gravity",
                           13: "LED tab"}

        for i in range(nbr + 1):
            name = "{}: ".format(i)
            if i in hardcoded_names:
                name += hardcoded_names[i]
            else:
                name += "N/A"
            self._led_ring_effect.addItem(name, i)

        self._led_ring_effect.currentIndexChanged.connect(
            self._ring_effect_changed)

        self._led_ring_effect.setCurrentIndex(current)
        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
            self._led_ring_effect.setEnabled(True)
            self._led_ring_headlight.setEnabled(True)

    def _ring_effect_changed(self, index):
        self._ring_effect = index
        if index > -1:
            i = self._led_ring_effect.itemData(index)
            logger.info("Changed effect to {}".format(i))
            if i != int(self.helper.cf.param.values["ring"]["effect"]):
                self.helper.cf.param.set_value("ring.effect", str(i))

    def _ring_effect_updated(self, name, value):
        if self.helper.cf.param.is_updated:
            self._led_ring_effect.setCurrentIndex(int(value))

    def _populate_assisted_mode_dropdown(self):
        self._assist_mode_combo.addItem("Altitude hold", 0)
        self._assist_mode_combo.addItem("Position hold", 1)
        self._assist_mode_combo.addItem("Height hold", 2)
        self._assist_mode_combo.addItem("Hover", 3)
        heightHoldPossible = False
        hoverPossible = False

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x09):
            heightHoldPossible = True

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x0A):
            heightHoldPossible = True
            hoverPossible = True

        if not heightHoldPossible:
            self._assist_mode_combo.model().item(2).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        if not hoverPossible:
            self._assist_mode_combo.model().item(3).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        self._assist_mode_combo.currentIndexChanged.connect(
            self._assist_mode_changed)
        self._assist_mode_combo.setEnabled(True)

        try:
            assistmodeComboIndex = Config().get("assistedControl")
            if assistmodeComboIndex == 3 and not hoverPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and hoverPossible:
                self._assist_mode_combo.setCurrentIndex(3)
                self._assist_mode_combo.currentIndexChanged.emit(3)
            elif assistmodeComboIndex == 2 and not heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(2)
                self._assist_mode_combo.currentIndexChanged.emit(2)
            else:
                self._assist_mode_combo.setCurrentIndex(assistmodeComboIndex)
                self._assist_mode_combo.currentIndexChanged.emit(
                                                    assistmodeComboIndex)
        except KeyError:
            defaultOption = 0
            if hoverPossible:
                defaultOption = 3
            elif heightHoldPossible:
                defaultOption = 2
            self._assist_mode_combo.setCurrentIndex(defaultOption)
            self._assist_mode_combo.currentIndexChanged.emit(defaultOption)

    def _connected(self, link_uri):
        """Callback when the Crazyflie has been connected"""
        self._plot.removeAllDatasets()
        self._plot.set_title("")

    def _disconnected(self, link_uri):
        """Callback for when the Crazyflie has been disconnected"""
        self._model.beginResetModel()
        self._model.reset()
        self._model.endResetModel()
        self.dataSelector.setCurrentIndex(-1)
        self._previous_config = None
        self._started_previous = False

    def _log_data_signal_wrapper(self, ts, data, logconf):
        """Wrapper for signal"""

        # For some reason the *.emit functions are not
        # the same over time (?!) so they cannot be registered and then
        # removed as callbacks.
        self._log_data_signal.emit(ts, data, logconf)

    def _log_error_signal_wrapper(self, config, msg):
        """Wrapper for signal"""

        # For some reason the *.emit functions are not
        # the same over time (?!) so they cannot be registered and then
        # removed as callbacks.
        self._plotter_log_error_signal.emit(config, msg)

    def _selection_changed(self, i):
        """Callback from ComboBox when a new item has been selected"""

        # Check if we have disconnected
        if i < 0:
            return
        # First check if we need to stop the old block
        if self._started_previous and self._previous_config:
            logger.debug("Should stop config [%s], stopping!",
                         self._previous_config.name)
            self._previous_config.delete()

        # Remove our callback for the previous config
        if self._previous_config:
            self._previous_config.data_received_cb.remove_callback(
                self._log_data_signal_wrapper)
            self._previous_config.error_cb.remove_callback(
                self._log_error_signal_wrapper)

        lg = self._model.get_config(i)
        if not lg.started:
            logger.debug("Config [%s] not started, starting!", lg.name)
            self._started_previous = True
            lg.start()
        else:
            self._started_previous = False
        self._plot.removeAllDatasets()
        color_selector = 0

        self._plot.set_title(lg.name)

        for d in lg.variables:
            self._plot.add_curve(d.name, self.colors[
                color_selector % len(self.colors)])
            color_selector += 1
        lg.data_received_cb.add_callback(self._log_data_signal_wrapper)
        lg.error_cb.add_callback(self._log_error_signal_wrapper)

        self._previous_config = lg

    def _config_added(self, logconfig):
        """Callback from the log layer when a new config has been added"""
        logger.debug("Callback for new config [%s]", logconfig.name)
        self._model.add_block(logconfig)

    def _plotter_logging_error(self, log_conf, msg):
        """Callback from the log layer when an error occurs"""
        QMessageBox.about(
            self, "Plot error", "Error when starting log config [%s]: %s" % (
                log_conf.name, msg))

    def _log_data_received(self, timestamp, data, logconf):
        """Callback when the log layer receives new data"""

        # Check so that the incoming data belongs to what we are currently
        # logging
        if self._previous_config:
            if self._previous_config.name == logconf.name:
                self._plot.add_data(data, timestamp)
예제 #3
0
class FlightTab(Tab, flight_tab_class):
    uiSetupReadySignal = pyqtSignal()
    _accel_data_signal = pyqtSignal(int, object, object)
    _gyro_data_signal = pyqtSignal(int, object, object)
    _motor_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _althold_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)

    _log_error_signal = pyqtSignal(object, str)

    #UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    def __init__(self, tabWidget, helper, *args):
        global D
        D.thrust = 0
        D.pitch = 0
        D.yawrate = 0
        D.roll = 0
        rospy.init_node("cf_flightTab")
        self.motor_pub = rospy.Publisher("cf_motorData", MotorData)
        self.stab_pub = rospy.Publisher("cf_stabData", StabData)
        self.acc_pub = rospy.Publisher("cf_accData", AccelData)
        self.gyro_pub = rospy.Publisher("cf_gyroData", GyroData)
        rospy.Subscriber("cf_textcmd", String, self._cmdCB)
        
        
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
                                     self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
                                     self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
                                     self._emergency_stop_updated_signal.emit)
        
        self.helper.inputDeviceReader.althold_updated.add_callback(
                    lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
        self._gyro_data_signal.connect(self._gyro_data_received)
        self._accel_data_signal.connect(self._accel_data_received)
        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.clientXModeCheckbox.setChecked(GuiConfig().get("client_side_xmode"))

        self.crazyflieXModeCheckbox.clicked.connect(
                             lambda enabled:
                             self.helper.cf.param.set_value("flightmode.x",
                                                            str(enabled)))
        self.helper.cf.param.add_update_callback(
                        group="flightmode", name="xmode",
                        cb=( lambda name, checked:
                        self.crazyflieXModeCheckbox.setChecked(eval(checked))))
        self.ratePidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(enabled)))
        self.angularPidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(not enabled)))
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="ratepid",
                    cb=(lambda name, checked:
                    self.ratePidRadioButton.setChecked(eval(checked))))
        
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="althold",
                    cb=(lambda name, enabled:
                    self.helper.inputDeviceReader.setAltHold(eval(enabled))))

        self.helper.cf.param.add_update_callback(
                        group="imu_sensors",
                        cb=self._set_available_sensors)
                
        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000,1])

        self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
        self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
                             GuiConfig().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error", "Error when starting log config"
                " [%s]: %s" % (log_conf.name, msg))
    
    def _cmdCB(self, data):
        m = data.data
        print ("Recieved command: " + m)
        if m is 'q':
            self.helper.cf.commander.send_setpoint(0, 0, 0, 0)
        elif m.count(" ")>0:
            hpr,value=m.split(" ")
            if(hpr is "pitch" or hpr is "p"):
                D.pitch= float(value)
            if(hpr=="yawrate" or hpr is "y"):
                D.yawrate= float(value)
            if(hpr=="roll" or hpr is "r"):
                D.roll=float(value)
            if(hpr=="thrust" or hpr is "t"):
                D.thrust= int(value)
            print (D.thrust)
            if D.thrust <= 10000:
                D.thrust = 10001
            elif D.thrust > 60000:
                D.thrust = 60000
            self.helper.cf.commander.send_setpoint(D.roll, D.pitch, D.yawrate, D.thrust)

    def _motor_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])
            d = MotorData()
            d.m1 = data["motor.m1"]
            d.m2 = data["motor.m2"]
            d.m3 = data["motor.m3"]
            d.m4 = data["motor.m4"]
            self.motor_pub.publish(d)

    def _gyro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
             d = GyroData()
             d.x = data["gyro.x"]
             d.y = data["gyro.y"]
             d.z = data["gyro.z"]
             self.gyro_pub.publish(d)
   
    def _accel_data_received(self, timestamp, data, logconf):
         if self.isVisible():
             d = AccelData() 
             d.x = data["acc.x"]
             d.y = data["acc.y"]
             d.z = data["acc.z"]
             self.acc_pub.publish(d)

    def _baro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
            self.ai.setBaro(data["baro.aslLong"])
        
    def _althold_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            target = data["altHold.target"]
            if target>0:
                if not self.targetASL.isEnabled():
                    self.targetASL.setEnabled(True) 
                self.targetASL.setText(("%.2f" % target))
                self.ai.setHover(target)    
            elif self.targetASL.isEnabled():
                self.targetASL.setEnabled(False)
                self.targetASL.setText("Not set")   
                self.ai.setHover(0)    
        
    def _imu_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText("%.2f%%" %
                                      self.thrustToPercentage(
                                                      data["stabilizer.thrust"]))
    
            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"])
            d = StabData()
            d.pitch = data["stabilizer.pitch"]
            d.roll = data["stabilizer.roll"]
            d.yaw = data["stabilizer.yaw"]
            self.stab_pub.publish(d)

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabalizer", GuiConfig().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")

        self.helper.cf.log.add_config(lg)
        if (lg.valid):
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")

        # MOTOR
        lg = LogConfig("Motors", GuiConfig().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
        
        #Accel
        lg = LogConfig("Accelerometer", GuiConfig().get("ui_update_period"))
        lg.add_variable("acc.x")
        lg.add_variable("acc.y")
        lg.add_variable("acc.z")
        
        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._accel_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")

        #Gyroscope
        lg = LogConfig("Gyroscope", GuiConfig().get("ui_update_period"))
        lg.add_variable("gyro.x")
        lg.add_variable("gyro.y")
        lg.add_variable("gyro.z")
        
        self.helper.cf.log.add_config(lg)
        if lg.valid:
            print ("Gyro logging started correctly")
            lg.data_received_cb.add_callback(self._gyro_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
            
    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
            else:
                self.actualASL.setEnabled(True)
                self.helper.inputDeviceReader.setAltHoldAvailable(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    self.logBaro = LogConfig("Baro", 200)
                    self.logBaro.add_variable("baro.aslLong", "float")

                    self.helper.cf.log.add_config(self.logBaro)
                    if self.logBaro.valid:
                        self.logBaro.data_received_cb.add_callback(
                            self._baro_data_signal.emit)
                        self.logBaro.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logBaro.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")            
                    self.logAltHold = LogConfig("AltHold", 200)
                    self.logAltHold.add_variable("altHold.target", "float")

                    self.helper.cf.log.add_config(self.logAltHold)
                    if self.logAltHold.valid:
                        self.logAltHold.data_received_cb.add_callback(
                            self._althold_data_signal.emit)
                        self.logAltHold.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logAltHold.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")                        

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.set_thrust_limits(
                            self.minThrust.value(), self.maxThrust.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("min_thrust", self.minThrust.value())
            GuiConfig().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.set_thrust_slew_limiting(
                            self.thrustLoweringSlewRateLimit.value(),
                            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("slew_limit", self.slewEnableLimit.value())
            GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_pitch(value)
        GuiConfig().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_roll(value)
        GuiConfig().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(("%0.2f %%" %
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                      self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
        logger.info("Changed flightmode to %s",
                    self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
            self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
            self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                              GuiConfig().get("normal_slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(GuiConfig().get("max_rp"))
            self.maxThrust.setValue(GuiConfig().get("max_thrust"))
            self.minThrust.setValue(GuiConfig().get("min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                                  GuiConfig().get("slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        GuiConfig().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)
예제 #4
0
class FlightTab(Tab, flight_tab_class):

    uiSetupReadySignal = pyqtSignal()

    _motor_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _althold_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)

    _log_error_signal = pyqtSignal(object, str)

    #UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    _limiting_updated = pyqtSignal(bool, bool, bool)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
                                     self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
                                     self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
                                     self._emergency_stop_updated_signal.emit)
        
        self.helper.inputDeviceReader.althold_updated.add_callback(
                    lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))

        self.crazyflieXModeCheckbox.clicked.connect(
                             lambda enabled:
                             self.helper.cf.param.set_value("flightmode.x",
                                                            str(enabled)))
        self.helper.cf.param.add_update_callback(
                        group="flightmode", name="xmode",
                        cb=( lambda name, checked:
                        self.crazyflieXModeCheckbox.setChecked(eval(checked))))

        self.ratePidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(enabled)))

        self.angularPidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(not enabled)))

        self._led_ring_headlight.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("ring.headlightEnable",
                                                   str(enabled)))

        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="ratepid",
                    cb=(lambda name, checked:
                    self.ratePidRadioButton.setChecked(eval(checked))))

        self.helper.cf.param.add_update_callback(
                    group="cpu", name="flash",
                    cb=self._set_enable_client_xmode)

        self.helper.cf.param.add_update_callback(
                    group="ring", name="headlightEnable",
                    cb=(lambda name, checked:
                    self._led_ring_headlight.setChecked(eval(checked))))

        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="althold",
                    cb=(lambda name, enabled:
                    self.helper.inputDeviceReader.enable_alt_hold(eval(enabled))))

        self._ledring_nbr_effects = 0

        self.helper.cf.param.add_update_callback(
                        group="ring",
                        name="neffect",
                        cb=(lambda name, value: self._set_neffect(eval(value))))

        self.helper.cf.param.add_update_callback(
                        group="imu_sensors",
                        cb=self._set_available_sensors)

        self.helper.cf.param.all_updated.add_callback(self._ring_populate_dropdown)

        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000,1])

        self.targetCalPitch.setValue(Config().get("trim_pitch"))
        self.targetCalRoll.setValue(Config().get("trim_roll"))

        self.helper.inputDeviceReader.alt1_updated.add_callback(self.alt1_updated)
        self.helper.inputDeviceReader.alt2_updated.add_callback(self.alt2_updated)
        self._tf_state = 0
        self._ring_effect = 0

        # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
        self.helper.inputDeviceReader.limiting_updated.add_callback(
            self._limiting_updated.emit)
        self._limiting_updated.connect(self._set_limiting_enabled)

    def _set_enable_client_xmode(self, name, value):
        if eval(value) <= 128:
            self.clientXModeCheckbox.setEnabled(True)
        else:
            self.clientXModeCheckbox.setEnabled(False)
            self.clientXModeCheckbox.setChecked(False)

    def _set_limiting_enabled(self, rp_limiting_enabled,
                                    yaw_limiting_enabled,
                                    thrust_limiting_enabled):
        self.maxAngle.setEnabled(rp_limiting_enabled)
        self.targetCalRoll.setEnabled(rp_limiting_enabled)
        self.targetCalPitch.setEnabled(rp_limiting_enabled)
        self.maxYawRate.setEnabled(yaw_limiting_enabled)
        self.maxThrust.setEnabled(thrust_limiting_enabled)
        self.minThrust.setEnabled(thrust_limiting_enabled)
        self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
        self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)

    def _set_neffect(self, n):
        self._ledring_nbr_effects = n

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
                             Config().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error", "Error when starting log config"
                " [%s]: %s" % (log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])
        
    def _baro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
            self.ai.setBaro(data["baro.aslLong"])
        
    def _althold_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            target = data["altHold.target"]
            if target>0:
                if not self.targetASL.isEnabled():
                    self.targetASL.setEnabled(True) 
                self.targetASL.setText(("%.2f" % target))
                self.ai.setHover(target)    
            elif self.targetASL.isEnabled():
                self.targetASL.setEnabled(False)
                self.targetASL.setText("Not set")   
                self.ai.setHover(0)    
        
    def _imu_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText("%.2f%%" %
                                      self.thrustToPercentage(
                                                      data["stabilizer.thrust"]))
    
            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"])

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        # MOTOR
        lg = LogConfig("Motors", Config().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
            self._led_ring_effect.setEnabled(True)
            self._led_ring_headlight.setEnabled(True)

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
            else:
                self.actualASL.setEnabled(True)
                self.helper.inputDeviceReader.set_alt_hold_available(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    self.logBaro = LogConfig("Baro", 200)
                    self.logBaro.add_variable("baro.aslLong", "float")

                    try:
                        self.helper.cf.log.add_config(self.logBaro)
                        self.logBaro.data_received_cb.add_callback(
                                self._baro_data_signal.emit)
                        self.logBaro.error_cb.add_callback(
                                self._log_error_signal.emit)
                        self.logBaro.start()
                    except KeyError as e:
                        logger.warning(str(e))
                    except AttributeError as e:
                        logger.warning(str(e))
                    self.logAltHold = LogConfig("AltHold", 200)
                    self.logAltHold.add_variable("altHold.target", "float")

                    try:
                        self.helper.cf.log.add_config(self.logAltHold)
                        self.logAltHold.data_received_cb.add_callback(
                            self._althold_data_signal.emit)
                        self.logAltHold.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logAltHold.start()
                    except KeyError as e:
                        logger.warning(str(e))
                    except AttributeError:
                        logger.warning(str(e))

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.clientXModeCheckbox.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None
        self._led_ring_effect.setEnabled(False)
        self._led_ring_headlight.setEnabled(False)


    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
        self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
        if (self.isInCrazyFlightmode == True):
            Config().set("min_thrust", self.minThrust.value())
            Config().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.thrust_slew_rate = self.thrustLoweringSlewRateLimit.value()
        self.helper.inputDeviceReader.thrust_slew_limit = self.slewEnableLimit.value()
        if (self.isInCrazyFlightmode == True):
            Config().set("slew_limit", self.slewEnableLimit.value())
            Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
        if (self.isInCrazyFlightmode == True):
            Config().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
        if (self.isInCrazyFlightmode == True):
            Config().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_pitch = value
        Config().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_roll = value
        Config().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(("%0.2f %%" %
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                      self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
        logger.debug("Changed flightmode to %s",
                    self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(Config().get("normal_max_rp"))
            self.maxThrust.setValue(Config().get("normal_max_thrust"))
            self.minThrust.setValue(Config().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                              Config().get("normal_slew_rate"))
            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(Config().get("max_rp"))
            self.maxThrust.setValue(Config().get("max_thrust"))
            self.minThrust.setValue(Config().get("min_thrust"))
            self.slewEnableLimit.setValue(Config().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                                  Config().get("slew_rate"))
            self.maxYawRate.setValue(Config().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        Config().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)

    def alt1_updated(self, state):
        if state:
            self._ring_effect += 1
            if self._ring_effect > self._ledring_nbr_effects:
                self._ring_effect = 0
            self.helper.cf.param.set_value("ring.effect", str(self._ring_effect))

    def alt2_updated(self, state):
        self.helper.cf.param.set_value("ring.headlightEnable", str(state))

    def _ring_populate_dropdown(self):
        try:
            nbr = int(self.helper.cf.param.values["ring"]["neffect"])
            current = int(self.helper.cf.param.values["ring"]["effect"])
        except KeyError:
            return

        hardcoded_names = {0: "Off",
                           1: "White spinner",
                           2: "Color spinner",
                           3: "Tilt effect",
                           4: "Brightness effect",
                           5: "Color spinner 2",
                           6: "Double spinner",
                           7: "Solid color effect",
                           8: "Factory test",
                           9: "Battery status",
                           10: "Boat lights",
                           11: "Alert",
                           12: "Gravity"}

        for i in range(nbr+1):
            name = "{}: ".format(i)
            if i in hardcoded_names:
                name += hardcoded_names[i]
            else:
                name += "N/A"
            self._led_ring_effect.addItem(name, QVariant(i))

        self._led_ring_effect.setCurrentIndex(current)
        self._led_ring_effect.currentIndexChanged.connect(self._ring_effect_changed)
        self.helper.cf.param.add_update_callback(group="ring",
                                         name="effect",
                                         cb=self._ring_effect_updated)

    def _ring_effect_changed(self, index):
        i = self._led_ring_effect.itemData(index).toInt()[0]
        logger.info("Changed effect to {}".format(i))
        if i != self.helper.cf.param.values["ring"]["effect"]:
            self.helper.cf.param.set_value("ring.effect", str(i))

    def _ring_effect_updated(self, name, value):
        self._led_ring_effect.setCurrentIndex(int(value))
class FlightTab(Tab, flight_tab_class):

    uiSetupReadySignal = pyqtSignal()

    _motor_data_signal = pyqtSignal(object, int)
    _imu_data_signal = pyqtSignal(object, int)
    _althold_data_signal = pyqtSignal(object, int)
    _baro_data_signal = pyqtSignal(object, int)
    _heading_data_signal = pyqtSignal(object, int)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)

    _log_error_signal = pyqtSignal(object, str)

    #UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connectSetupFinished.add_callback(
                                     self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
                                     self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
                                     self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
                                     self._emergency_stop_updated_signal.emit)
        
        self.helper.inputDeviceReader.althold_updated.add_callback(
                    lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)
        self._heading_data_signal.connect(self._heading_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.clientXModeCheckbox.setChecked(GuiConfig().get("client_side_xmode"))

        self.crazyflieXModeCheckbox.clicked.connect(
                             lambda enabled:
                             self.helper.cf.param.set_value("flightmode.x",
                                                            str(enabled)))
        self.helper.cf.param.add_update_callback(
                        group="flightmode", name="xmode",
                        cb=( lambda name, checked:
                        self.crazyflieXModeCheckbox.setChecked(eval(checked))))
        self.ratePidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(enabled)))
        self.angularPidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(not enabled)))
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="ratepid",
                    cb=(lambda name, checked:
                    self.ratePidRadioButton.setChecked(eval(checked))))
        
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="althold",
                    cb=(lambda name, enabled:
                    self.helper.inputDeviceReader.setAltHold(eval(enabled))))

        self.helper.cf.param.add_update_callback(
                        group="imu_sensors",
                        cb=self._set_available_sensors)
                
        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        #self.verticalLayout_4.addWidget(self.ai)
        #self.splitter.setSizes([1000,1])
        self.gridLayout.addWidget(self.ai, 0, 1)
        
        self.compass = Compass()
        self.gridLayout.addWidget(self.compass, 0, 2)

        self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
        self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
        
        self.apitch = 0
        self.aroll = 0
        self.motor_power = 0

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
                             GuiConfig().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error", "Error when starting log config"
                " [%s]: %s" % (log_conf.getName(), msg))

    def _motor_data_received(self, data, timestamp):
        self.actualM1.setValue(data["motor.m1"])
        self.actualM2.setValue(data["motor.m2"])
        self.actualM3.setValue(data["motor.m3"])
        self.actualM4.setValue(data["motor.m4"])
        self.motor_power = data["motor.m1"] + data["motor.m2"] + data["motor.m3"] + data["motor.m4"]
        
    def _baro_data_received(self, data, timestamp):
        self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
        self.ai.setBaro(data["baro.aslLong"])
        
    def _althold_data_received(self, data, timestamp):       
        target = data["altHold.target"]
        if target>0:
            if not self.targetASL.isEnabled():
                self.targetASL.setEnabled(True) 
            self.targetASL.setText(("%.2f" % target))
            self.ai.setHover(target)    
        elif self.targetASL.isEnabled():
            self.targetASL.setEnabled(False)
            self.targetASL.setText("Not set")   
            self.ai.setHover(0)    
        
    def _imu_data_received(self, data, timestamp):
        self.aroll = data["stabilizer.roll"]
        self.apitch = data["stabilizer.pitch"]
        self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
        self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
        self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
        self.actualThrust.setText("%.2f%%" %
                                  self.thrustToPercentage(
                                                  data["stabilizer.thrust"]))

        self.ai.setRollPitch(-data["stabilizer.roll"],
                             data["stabilizer.pitch"])
                           
    def _heading_data_received(self, data, timestamp):
    # hard and soft correction values generated by Processing Magnetometer_calibration script + calibrate_hardsoft.py
        # magn_ellipsoid_center = [1341.66, -537.690, 41.1584]
        # magn_ellipsoid_transform = [[0.934687, 0.0222809, 0.0151145], [0.0222809, 0.919365, -0.00622367], [0.0151145, -0.00622367, 0.996487]]                                
        magn_ellipsoid_center = [-170.956, -1056.30, 19.4655]
        magn_ellipsoid_transform = [[0.852266, 0.00526498, 0.0195745], [0.00526498, 0.888268, -0.00355351], [0.0195745, -0.00355351, 0.997333]]


        # values generated by calibrate_powered.py
        qx = [0.067946222436498283, -0.25034004667098259, 8.3336994198409666, -0.17762637163222378]
        qy = [-0.13945102271766135, 2.9074808469097495, 1.6764850422889934, 0.19244505046927501]
        qz = [0.018800599305554239, -0.79590273035713055, -3.1033531112103478, 0.13550993988096199]
                
        # matrix by vector multiplication
        def mv(a, b):
            out = [0,0,0]
            for x in range(0, 3):
                out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];
            return out 
        
        # calculate adjustments related to how much power is sent to the motors      
        def adj(qs, power):
            p = float(power) / float(40000) # 10k * 4 motors
            return qs[0]*p**3+qs[1]*p**2+qs[2]*p+qs[3]

        x, y, z = data['heading.x'], data['heading.y'], data['heading.z']

        x = x - magn_ellipsoid_center[0]
        y = y - magn_ellipsoid_center[1]
        z = z - magn_ellipsoid_center[2]
        x, y, z = mv(magn_ellipsoid_transform, [x, y, z])

        x = x + adj(qx, self.motor_power)
        y = y + adj(qy, self.motor_power)
        z = z + adj(qz, self.motor_power)
        
        # correct magnetometer orientation relative to the CF orientation
        x, y, z = y, x, z * -1

        # calculate tilt-compensated heading angle        
        cosRoll = cos(math.radians(self.aroll))
        sinRoll = sin(math.radians(self.aroll))  
        cosPitch = cos(math.radians(self.apitch))
        sinPitch = sin(math.radians(self.apitch))
  
        Xh = x * cosPitch + z * sinPitch
        Yh = x * sinRoll * sinPitch + y * cosRoll - z * sinRoll * cosPitch
  
        heading = math.atan2(Yh, Xh)
        d_heading = math.degrees(heading) * -1 # for some reason getting inveted sign here
        JoystickReader.controller.getCFHeading(d_heading)
        
        # update compass widget
        self.compass.setAngle(d_heading)

        # lock heading to 0 degrees north
        if d_heading > 0:
            yaw_trim = -20
        else:
            yaw_trim = 20
        self.helper.inputDeviceReader.update_trim_yaw_signal.emit(yaw_trim)

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabalizer", 200)
        lg.addVariable(LogVariable("stabilizer.roll", "float"))
        lg.addVariable(LogVariable("stabilizer.pitch", "float"))
        lg.addVariable(LogVariable("stabilizer.yaw", "float"))
        lg.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))

        self.log = self.helper.cf.log.create_log_packet(lg)
        if (self.log is not None):
            self.log.data_received.add_callback(self._imu_data_signal.emit)
            self.log.error.add_callback(self._log_error_signal.emit)
            self.log.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")

        # MOTOR
        lg = LogConfig("Motors", 200)
        lg.addVariable(LogVariable("motor.m1", "uint32_t"))
        lg.addVariable(LogVariable("motor.m2", "uint32_t"))
        lg.addVariable(LogVariable("motor.m3", "uint32_t"))
        lg.addVariable(LogVariable("motor.m4", "uint32_t"))

        self.log = self.helper.cf.log.create_log_packet(lg)
        if (self.log is not None):
            self.log.data_received.add_callback(self._motor_data_signal.emit)
            self.log.error.add_callback(self._log_error_signal.emit)
            self.log.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
                        
        # HEADING
        lg = LogConfig("Heading", 200)
        lg.addVariable(LogVariable("heading.x", "int16_t"))
        lg.addVariable(LogVariable("heading.y", "int16_t"))
        lg.addVariable(LogVariable("heading.z", "int16_t"))

        self.log = self.helper.cf.log.create_log_packet(lg)
        if (self.log is not None):
            self.log.data_received.add_callback(self._heading_data_signal.emit)
            self.log.error.add_callback(self._log_error_signal.emit)
            self.log.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
            
    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
            else:
                self.actualASL.setEnabled(True)
                self.helper.inputDeviceReader.setAltHoldAvailable(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    lg = LogConfig("Baro", 200)
                    lg.addVariable(LogVariable("baro.aslLong", "float"))

                    self.logBaro = self.helper.cf.log.create_log_packet(lg)
                    if (self.logBaro is not None):
                        self.logBaro.data_received.add_callback(self._baro_data_signal.emit)
                        self.logBaro.error.add_callback(self._log_error_signal.emit)
                        self.logBaro.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")            
                    lg = LogConfig("AltHold", 200)
                    lg.addVariable(LogVariable("altHold.target", "float"))

                    self.logAltHold = self.helper.cf.log.create_log_packet(lg)
                    if (self.logAltHold is not None):
                        self.logAltHold.data_received.add_callback(self._althold_data_signal.emit)
                        self.logAltHold.error.add_callback(self._log_error_signal.emit)
                        self.logAltHold.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")                        

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.set_thrust_limits(
                            self.minThrust.value(), self.maxThrust.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("min_thrust", self.minThrust.value())
            GuiConfig().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.set_thrust_slew_limiting(
                            self.thrustLoweringSlewRateLimit.value(),
                            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("slew_limit", self.slewEnableLimit.value())
            GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_pitch(value)
        GuiConfig().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_roll(value)
        GuiConfig().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(("%0.2f %%" %
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.actualM1.setEnabled(enabled)
        self.actualM2.setEnabled(enabled)
        self.actualM3.setEnabled(enabled)
        self.actualM4.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                      self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
        logger.info("Changed flightmode to %s",
                    self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
            self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
            self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                              GuiConfig().get("normal_slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(GuiConfig().get("max_rp"))
            self.maxThrust.setValue(GuiConfig().get("max_thrust"))
            self.minThrust.setValue(GuiConfig().get("min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                                  GuiConfig().get("slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        GuiConfig().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)
class FlightTab(Tab, flight_tab_class):

    uiSetupReadySignal = pyqtSignal()
    
    _mode_index = 0

    _motor_data_signal = pyqtSignal(int, object, object)
    _acc_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _althold_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)
    _mag_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)
    _switch_mode_updated_signal = pyqtSignal()

    _log_error_signal = pyqtSignal(object, str)
    
    # UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)
        
        self.setStyleSheet("QToolTip { color: #ffffff; background-color: #2d2d2d; border: 1px solid #f0f0f0; border-radius: 3px; }")
        
        self.apitch = 0
        self.aroll = 0
        self.motor_power = 0
        self.tabName = "Flight Control"
        self.menuName = "Flight Control"
        
        self.ai = AttitudeIndicator()
        self.compass = CompassWidget()

        self.tabWidget = tabWidget
        self.helper = helper
        
        ########## Freefall related stuff          
        self.ffd = FFD(parent=self) # detection class          
        self.ffr = FFR(parent=self) # recovery class          
               
        # Connect the crash/free fall detections          
        self.ffd.sigFreeFall.connect(self.freefallDetected)          
        self.ffd.sigCrashed.connect(self.crashDetected)          
        self.ffr.sigRecoveryTimedOut.connect(self.recoveryAborted)          
               
        # continuously send acc, mean, var to AI          
        self.ffd.sigAccMeanVar.connect(self.ai.setFFAccMeanVar)          
        # Only set recovery on if both fall and crash detection are on as well as recovery          
        self.checkBox_ffd.stateChanged.connect(lambda on: self.ffGuiSync(0,on) )  # detect freefall on/off          
        self.checkBox_crash.stateChanged.connect(lambda on: self.ffGuiSync(1,on)) # detect crashing on/off          
        self.checkBox_ffr.stateChanged.connect(lambda on: self.ffGuiSync(2,on))   # recovery crashing on/off          
               
        # Use barometer for recovery (only clickedable if the checkbox activates in reaction to detecting a 10DOF flie          
        self.checkBox_ffbaro.clicked.connect(self.ffr.setUseBaro)          
               
        # intercept control commands          
        self.helper.inputDeviceReader.auto_input_updated.add_callback(self.ffr.step)          
        self.ffr.auto_input_updated.add_callback(self.helper.cf.commander.send_setpoint)          
        self.ffr.auto_input_updated.add_callback(self._input_updated_signal.emit)          
        self.ffr.althold_updated.add_callback(lambda param, arg: self.helper.cf.param.set_value(param, str(arg)))          
               
        # Emergency Stop          
        self._emergency_stop_updated_signal.connect(self.ffr.setKillSwitch)          
        #self._emergency_stop_updated_signal.connect(self.ai.setKillSwitch)          
               
        # Debugging Freefall          
        self.pushButton_ff.clicked.connect(self.ffd.sendFakeEmit)          
        self.pushButton_crash.clicked.connect(self.ffd.sendFakeLandingEmit)          
        self.doubleSpinBox_ff_falloff.valueChanged.connect(self.ffr.falloff.setWidth)          
        self.doubleSpinBox_ff_max.valueChanged.connect(self.ffr.falloff.setMaxThrust)          
        self.doubleSpinBox_ff_min.valueChanged.connect(self.ffr.falloff.setMinThrust)          
        self.doubleSpinBox_ff_time.valueChanged.connect(self.ffr.falloff.setTime)          
        self.pushButton_plot.clicked.connect(self.ffr.falloff.plot)          
               
        self.checkBox_debug.stateChanged.connect(self.toggleFFDebug)          
        self.checkBox_ffbaro.clicked.connect(self.toggleFFDebug)          
               
        # Slow down drawing to GUi items by keeping track of last received data time          
        self.lastImuTime = 0 

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
                                     self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
                                     self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(self._emergency_stop_updated_signal.emit)
        
        self._switch_mode_updated_signal.connect(self.switchMode)
        self.helper.inputDeviceReader.switch_mode_updated.add_callback(self._switch_mode_updated_signal.emit)
        
        self.helper.inputDeviceReader.althold_updated.add_callback(self.changeHoldmode)

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)
        self._acc_data_signal.connect(self._acc_data_received)
        self._mag_data_signal.connect(self._mag_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxAltitude.valueChanged.connect(self.maxAltitudeChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
                                      self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientHoldModeCheckbox.toggled.connect(self.changeHoldmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()
        
        self.viscousModeSlider.valueChanged.connect(self.setViscousThrust)
        self.clientHoldModeCheckbox.setChecked(GuiConfig().get("client_side_holdmode"))

        # self.crazyflieXModeCheckbox.clicked.connect(
        #                     lambda enabled:
        #                     self.helper.cf.param.set_value("flightmode.x", str(enabled)))
        # self.helper.cf.param.add_update_callback(
        #                group="flightmode", name="xmode",
        #                cb=( lambda name, checked:
        #                self.crazyflieXModeCheckbox.setChecked(eval(checked))))
        self.ratePidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(enabled)))
        self.angularPidRadioButton.clicked.connect(
                    lambda enabled:
                    self.helper.cf.param.set_value("flightmode.ratepid",
                                                   str(not enabled)))
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="ratepid",
                    cb=(lambda name, checked:
                    self.ratePidRadioButton.setChecked(eval(checked))))
        
        self.helper.cf.param.add_update_callback(
                    group="flightmode", name="althold",
                    cb=(lambda name, enabled:
                    self.helper.inputDeviceReader.setAltHold(eval(enabled))))

        self.helper.cf.param.add_update_callback(
                        group="imu_sensors",
                        cb=self._set_available_sensors)
                
        self.logBaro = None
        self.logAltHold = None

        self.horizontalLayout_5.addWidget(self.ai)
        self.horizontalLayout_5.addWidget(self.compass)
        # self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000, 1])
        

        self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
        self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
        
    def setViscousThrust(self, thrust):
        self.helper.inputDeviceReader.setViscousModeThrust(thrust)
        point = QtGui.QCursor.pos()
        QtGui.QToolTip.showText(QtCore.QPoint(point.x(), self.viscousModeSlider.pos().y()*2-12), QtCore.QString.number(thrust) + "%")
    
    @pyqtSlot(int)          
    def toggleFFDebug(self, ignored=None):          
            if self.checkBox_debug.isChecked():          
                self.pushButton_ff.show()          
                self.pushButton_crash.show()          
               
                if self.checkBox_ffbaro.isEnabled() and self.checkBox_ffbaro.isChecked():          
                    self.pushButton_plot.hide()          
                    self.doubleSpinBox_ff_falloff.hide()          
                    self.doubleSpinBox_ff_max.hide()          
                    self.doubleSpinBox_ff_min.hide()          
                    self.doubleSpinBox_ff_time.hide()          
                    self.label_ff4.hide()          
                    self.label_ff1.hide()          
                    self.label_ff2.hide()          
                    self.label_ff3.hide()          
                else:          
                    self.pushButton_plot.show()          
                    self.doubleSpinBox_ff_falloff.show()          
                    self.doubleSpinBox_ff_max.show()          
                    self.doubleSpinBox_ff_min.show()          
                    self.doubleSpinBox_ff_time.show()          
                    self.label_ff4.show()          
                    self.label_ff1.show()          
                    self.label_ff2.show()          
                    self.label_ff3.show()          
            else:          
                self.pushButton_ff.hide()          
                self.pushButton_crash.hide()          
                self.pushButton_plot.hide()          
                self.doubleSpinBox_ff_falloff.hide()          
                self.doubleSpinBox_ff_max.hide()          
                self.doubleSpinBox_ff_min.hide()          
                self.doubleSpinBox_ff_time.hide()          
                self.label_ff4.hide()          
                self.label_ff1.hide()          
                self.label_ff2.hide()          
                self.label_ff3.hide()          
               
               
    @pyqtSlot()          
    def freefallDetected(self):          
            self.ai.setFreefall()          
            if self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled():          
                self.ffr.startRecovery()          
                self.ai.setRecovery(True)          
                self.helper.inputDeviceReader.setAuto(True)          
                self.checkBox_ffr.setChecked(False)          
                # self.emergency_stop_label.setText("Recovering Freefall")          
            # self.checkBox_ffr.setEnabled(False)          
               
    @pyqtSlot(float)          
    def crashDetected(self, badness):          
            self.ai.setRecovery(False, 'Landed / Crashed')          
            self.ai.setCrash(badness)          
            self.helper.inputDeviceReader.setAuto(False)          
            self.ffr.setLanded()          
            # self.emergency_stop_label.setText("")          
            self.checkBox_ffr.setChecked(True)          
               
    @pyqtSlot()          
    def recoveryAborted(self):          
            self.ai.setRecovery(False, 'Recovery Aborted / Timed out')          
            self.helper.inputDeviceReader.setAuto(False)          
            # self.emergency_stop_label.setText("")          
            # self.checkBox_ffr.setEnabled(True)          
            pass          
               
               
    def ffGuiSync(self, id, on):          
            """ Keeps freefall gui elements in sync"""          
            if   id == 0:  # freefall detection click          
                self.ffd.setEnabled(on)  # detect freefall on/off          
            elif id == 1:  # landing detection click          
                self.ffd.setEnabledLanding(on)  # detect crashing on/off          
            # elif id == 2: # recovery detection click          
               
               
            # Only allow recivery checkbox to be turned on if crash/freefall detection is active          
            self.checkBox_ffr.setEnabled(self.checkBox_ffd.isChecked() and self.checkBox_crash.isChecked())          
               
            # Only allow recovery class to be turned on if all boxes are checked          
            # self.ffr.setEnabled(on=self.checkBox_ffd.isChecked() and self.checkBox_crash.isChecked() and self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled())          
               
               
    def makeSpace(self):          
            # I wish I knew how to make the ai QWidget fullscreen....          
            s = QtGui.QSplitter()          
            if self.splitter_2.widget(0).isHidden():          
                self.splitter_2.widget(0).show()          
                self.splitter.widget(1).show()          
            else:          
                self.splitter_2.widget(0).hide()          
                self.splitter.widget(1).hide()          
               
    def _acc_data_received(self, timestamp, data, logconf): 
            self.ffd.readAcc(data["acc.zw"])    
            self.helper.cf.commander.setActualGravity(data)
                  
               
               
    def updateCamList(self):          
            """Repopulate the combobox with available camera devices and select the last one (=usually the camera we need)"""          
               
            # Remove all          
            self.comboBox_camera.clear()          
               
            # Query available devices          
            devices = self.cam.getDevices()          
            for i, device in enumerate(devices):          
                logger.debug("Camera device [%d]: %s", i, device)          
                self.comboBox_camera.addItem(device)          
               
            # enable/disable the on/off button and dropdown          
            self.checkBox_camera.setEnabled(len(devices) > 0)          
            self.comboBox_camera.setEnabled(len(devices) > 0)          
            if len(devices) == 0:          
                self.comboBox_camera.addItem("None Detected")          
               
            # Select last one (eg the one we just plugged in)          
            self.comboBox_camera.setCurrentIndex(max(0, len(devices) - 1)) 

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
                             GuiConfig().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error", "Error when starting log config"
                " [%s]: %s" % (log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        self.motor_power = data["motor.m1"] + data["motor.m2"] + data["motor.m3"] + data["motor.m4"]
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])
        
    def _baro_data_received(self, timestamp, data, logconf):
        self.helper.inputDeviceReader.setCurrentAltitude(data["baro.aslLong"])
        if self.isVisible():
            self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
            self.ai.setBaro(data["baro.aslLong"])
        
    def _althold_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            target = data["altHold.target"]
            if target > 0:
                if not self.targetASL.isEnabled():
                    self.targetASL.setEnabled(True) 
                self.targetASL.setText(("%.2f" % target))
                self.ai.setHover(target)    
            elif self.targetASL.isEnabled():
                self.targetASL.setEnabled(False)
                self.targetASL.setText("Not set")   
                self.ai.setHover(0)    
        
    def _imu_data_received(self, timestamp, data, logconf):
        self.aroll = data["stabilizer.roll"]  
        self.apitch = data["stabilizer.pitch"]
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText("%.2f%%" % 
                                      self.thrustToPercentage(
                                                      data["stabilizer.thrust"]))
    
            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"])
            self.compass.setAngle(-data["stabilizer.yaw"])
            # self.helper.cf.commander.setActualPoint(data)
            
    def _mag_data_received(self, timestamp, data, logconf):              
        # hard and soft correction values generated by Processing Magnetometer_calibration script + calibrate_hardsoft.py
        magn_ellipsoid_center = [1341.66, -537.690, 41.1584]
        magn_ellipsoid_transform = [[0.934687, 0.0222809, 0.0151145], [0.0222809, 0.919365, -0.00622367], [0.0151145, -0.00622367, 0.996487]]                                

        # values generated by calibrate_powered.py
        qx = [0.067946222436498283, -0.25034004667098259, 8.3336994198409666, -0.17762637163222378]
        qy = [-0.13945102271766135, 2.9074808469097495, 1.6764850422889934, 0.19244505046927501]
        qz = [0.018800599305554239, -0.79590273035713055, -3.1033531112103478, 0.13550993988096199]
                
        # matrix by vector multiplication
        def mv(a, b):
            out = [0,0,0]
            for x in range(0, 3):
                out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];
            return out 
        
        # calculate adjustments related to how much power is sent to the motors      
        def adj(qs, power):
            p = float(power) / float(40000) # 10k * 4 motors
            return qs[0]*p**3+qs[1]*p**2+qs[2]*p+qs[3]

        x, y, z = data['mag.x'], data['mag.y'], data['mag.z']

        x = x - magn_ellipsoid_center[0]
        y = y - magn_ellipsoid_center[1]
        z = z - magn_ellipsoid_center[2]
        x, y, z = mv(magn_ellipsoid_transform, [x, y, z])

        x = x + adj(qx, self.motor_power)
        y = y + adj(qy, self.motor_power)
        z = z + adj(qz, self.motor_power)
        
        # correct magnetometer orientation relative to the CF orientation
        x, y, z = y, x, z * -1

        # calculate tilt-compensated heading angle        
        cosRoll = cos(math.radians(self.aroll))
        sinRoll = sin(math.radians(self.aroll))  
        cosPitch = cos(math.radians(self.apitch))
        sinPitch = sin(math.radians(self.apitch))
  
        Xh = x * cosPitch + z * sinPitch
        Yh = x * sinRoll * sinPitch + y * cosRoll - z * sinRoll * cosPitch
  
        heading = math.atan2(Yh, Xh)
        d_heading = math.degrees(heading) * -1 # for some reason getting inveted sign here
        
        # update compass widget
        #self.compass.setAngle(d_heading)

        # lock heading to 0 degrees north
        if d_heading > 0:
            yaw_trim = -20
        else:
            yaw_trim = 20
        self.helper.inputDeviceReader.update_trim_yaw_signal(yaw_trim)

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabalizer", GuiConfig().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")
        lg.add_variable("acc.x", "float")
        lg.add_variable("acc.y", "float")
        lg.add_variable("acc.z", "float")

        self.helper.cf.log.add_config(lg)
        if (lg.valid):
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
            
        # MOTOR
        lg = LogConfig("Motors", GuiConfig().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")
        
        # Acc for freefall
        lg = LogConfig("Acc", 10) # Yes we need this at 100hz for freefall detection!!
        lg.add_variable("acc.zw", "float")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._acc_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after connection [ACC]!")
        
        lg = LogConfig("Magnetometer", 100)
        lg.add_variable("mag.x", "int16_t")
        lg.add_variable("mag.y", "int16_t")
        lg.add_variable("mag.z", "int16_t")
            
        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._mag_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after connection!")
            
    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.maxAltitudeLabel.setEnabled(False)
                self.maxAltitude.setEnabled(False)
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
                self.checkBox_ffbaro.setEnabled(False)       
                self.checkBox_ffbaro.setChecked(False) 
            else:
                self.actualASL.setEnabled(True)
                self.checkBox_ffbaro.setEnabled(True)
                self.helper.inputDeviceReader.setAltHoldAvailable(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    self.logBaro = LogConfig("Baro", 200)
                    self.logBaro.add_variable("baro.aslLong", "float")

                    self.helper.cf.log.add_config(self.logBaro)
                    if self.logBaro.valid:
                        self.logBaro.data_received_cb.add_callback(self._baro_data_signal.emit)
                        self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
                        self.logBaro.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")            
                    self.logAltHold = LogConfig("AltHold", 200)
                    self.logAltHold.add_variable("altHold.target", "float")

                    self.helper.cf.log.add_config(self.logAltHold)
                    if self.logAltHold.valid:
                        self.logAltHold.data_received_cb.add_callback(self._althold_data_signal.emit)
                        self.logAltHold.error_cb.add_callback(self._log_error_signal.emit)
                        self.logAltHold.start()
                    else:
                        logger.warning("Could not setup logconfiguration after "
                                       "connection!")                        

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.set_thrust_limits(self.minThrust.value(), self.maxThrust.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("min_thrust", self.minThrust.value())
            GuiConfig().set("max_thrust", self.maxThrust.value())
            
    def maxAltitudeChanged(self):
        self.helper.inputDeviceReader.setMaxAltitude(self.maxAltitude.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.set_thrust_slew_limiting(
                            self.thrustLoweringSlewRateLimit.value(),
                            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("slew_limit", self.slewEnableLimit.value())
            GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_pitch(value)
        GuiConfig().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_roll(value)
        GuiConfig().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(("%0.2f %%" % 
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setEnabled(True)
            self.emergency_stop_label.setText(self.emergencyStopStringWithText("Kill Switch Active"))
            self.ai.setKillSwitch(True)
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("Kill Swith")
            self.emergency_stop_label.setEnabled(False)
            self.ai.setKillSwitch(False)

    def flightmodeChange(self, item):
        GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
        logger.info("Changed flightmode to %s",
                    self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
            self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
            self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                              GuiConfig().get("normal_slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(GuiConfig().get("max_rp"))
            self.maxThrust.setValue(GuiConfig().get("max_thrust"))
            self.minThrust.setValue(GuiConfig().get("min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                                                  GuiConfig().get("slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)
        
    @pyqtSlot(bool)
    def changeCareFreemode(self, checked):
        # self.helper.cf.commander.set_client_carefreemode(checked)
        if checked:
            self.clientCareFreeModeRadio.setChecked(checked)
            self.helper.cf.param.set_value("FlightMode.flightmode", "0")
            # self.helper.cf.commander.set_client_positionmode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        elif not checked and self._mode_index == 1:
            self.clientCareFreeModeRadio.setChecked(False)
            self.clientNormalModeRadio.setChecked(True)
            self.helper.cf.param.set_value("FlightMode.flightmode", "1")
        logger.info("CareFree-mode enabled: %s", checked)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.clientXModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_xmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "2")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_positionmode(False)
        logger.info("X-mode enabled: %s", checked)
        
    @pyqtSlot(bool)
    def changePositionmode(self, checked):
        self.clientPositionModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_positionmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "3")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        logger.info("Position-mode enabled: %s", checked)
        
    @pyqtSlot(bool)
    def changeHeadingmode(self, checked):
        self.clientHeadingModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_positionmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "4")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        logger.info("Heading-mode enabled: %s", checked)
        
    @pyqtSlot(bool)
    def changeHoldmode(self, checked):
        self.clientHoldModeCheckbox.setChecked(checked)
        self.helper.cf.param.set_value("flightmode.althold", str(checked))
        # self.helper.cf.commander.set_client_holdmode(checked)
        GuiConfig().set("client_side_holdmode", checked)
        logger.info("Hold-mode enabled: %s", checked)
    
    def switchMode(self):
        if self._mode_index == 4:
            self._mode_index = 0 
        else: 
            self._mode_index += 1
        if self._mode_index == 0:
            self.changeCareFreemode(True)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 1:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 2:
            self.changeCareFreemode(False)
            self.changeXmode(True)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 3:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(True)
            self.changeHeadingmode(False)
        elif self._mode_index == 4:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(True)
        logger.info("Mode switched to index: %d", self._mode_index)
예제 #7
0
class FlightTab(Tab, flight_tab_class):

    uiSetupReadySignal = pyqtSignal()

    _motor_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _althold_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)

    _log_error_signal = pyqtSignal(object, str)

    #UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    _limiting_updated = pyqtSignal(bool, bool, bool)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
            self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
            self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
            self._emergency_stop_updated_signal.emit)

        self.helper.inputDeviceReader.althold_updated.add_callback(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.althold", enabled))

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientXModeCheckbox.toggled.connect(self.changeXmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))

        self.crazyflieXModeCheckbox.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.x", str(enabled)))
        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="xmode",
            cb=(lambda name, checked: self.crazyflieXModeCheckbox.setChecked(
                eval(checked))))

        self.ratePidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(enabled)))

        self.angularPidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(not enabled)))

        self._led_ring_headlight.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "ring.headlightEnable", str(enabled)))

        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="ratepid",
            cb=(lambda name, checked: self.ratePidRadioButton.setChecked(
                eval(checked))))

        self.helper.cf.param.add_update_callback(
            group="ring",
            name="headlightEnable",
            cb=(lambda name, checked: self._led_ring_headlight.setChecked(
                eval(checked))))

        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="althold",
            cb=(lambda name, enabled: self.helper.inputDeviceReader.
                enable_alt_hold(eval(enabled))))

        self._ledring_nbr_effects = 0

        self.helper.cf.param.add_update_callback(
            group="ring",
            name="neffect",
            cb=(lambda name, value: self._set_neffect(eval(value))))

        self.helper.cf.param.add_update_callback(
            group="imu_sensors", cb=self._set_available_sensors)

        self.helper.cf.param.all_updated.add_callback(
            self._ring_populate_dropdown)

        self.logBaro = None
        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000, 1])

        self.targetCalPitch.setValue(Config().get("trim_pitch"))
        self.targetCalRoll.setValue(Config().get("trim_roll"))

        self.helper.inputDeviceReader.alt1_updated.add_callback(
            self.alt1_updated)
        self.helper.inputDeviceReader.alt2_updated.add_callback(
            self.alt2_updated)
        self._tf_state = 0
        self._ring_effect = 0

        # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
        self.helper.inputDeviceReader.limiting_updated.add_callback(
            self._limiting_updated.emit)
        self._limiting_updated.connect(self._set_limiting_enabled)

    def _set_limiting_enabled(self, rp_limiting_enabled, yaw_limiting_enabled,
                              thrust_limiting_enabled):
        self.maxAngle.setEnabled(rp_limiting_enabled)
        self.targetCalRoll.setEnabled(rp_limiting_enabled)
        self.targetCalPitch.setEnabled(rp_limiting_enabled)
        self.maxYawRate.setEnabled(yaw_limiting_enabled)
        self.maxThrust.setEnabled(thrust_limiting_enabled)
        self.minThrust.setEnabled(thrust_limiting_enabled)
        self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
        self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)

    def _set_neffect(self, n):
        self._ledring_nbr_effects = n

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
            Config().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(
            self, "Log error", "Error when starting log config"
            " [%s]: %s" % (log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])

    def _baro_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
            self.ai.setBaro(data["baro.aslLong"])

    def _althold_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            target = data["altHold.target"]
            if target > 0:
                if not self.targetASL.isEnabled():
                    self.targetASL.setEnabled(True)
                self.targetASL.setText(("%.2f" % target))
                self.ai.setHover(target)
            elif self.targetASL.isEnabled():
                self.targetASL.setEnabled(False)
                self.targetASL.setText("Not set")
                self.ai.setHover(0)

    def _imu_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText(
                "%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))

            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"])

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabalizer", Config().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        # MOTOR
        lg = LogConfig("Motors", Config().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

        if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
            self._led_ring_effect.setEnabled(True)
            self._led_ring_headlight.setEnabled(True)

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
            else:
                self.actualASL.setEnabled(True)
                self.helper.inputDeviceReader.set_alt_hold_available(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    self.logBaro = LogConfig("Baro", 200)
                    self.logBaro.add_variable("baro.aslLong", "float")

                    try:
                        self.helper.cf.log.add_config(self.logBaro)
                        self.logBaro.data_received_cb.add_callback(
                            self._baro_data_signal.emit)
                        self.logBaro.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logBaro.start()
                    except KeyError as e:
                        logger.warning(str(e))
                    except AttributeError as e:
                        logger.warning(str(e))
                    self.logAltHold = LogConfig("AltHold", 200)
                    self.logAltHold.add_variable("altHold.target", "float")

                    try:
                        self.helper.cf.log.add_config(self.logAltHold)
                        self.logAltHold.data_received_cb.add_callback(
                            self._althold_data_signal.emit)
                        self.logAltHold.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logAltHold.start()
                    except KeyError as e:
                        logger.warning(str(e))
                    except AttributeError:
                        logger.warning(str(e))

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None
        self._led_ring_effect.setEnabled(False)
        self._led_ring_headlight.setEnabled(False)

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.set_thrust_limits(
            self.minThrust.value(), self.maxThrust.value())
        if (self.isInCrazyFlightmode == True):
            Config().set("min_thrust", self.minThrust.value())
            Config().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.set_thrust_slew_limiting(
            self.thrustLoweringSlewRateLimit.value(),
            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode == True):
            Config().set("slew_limit", self.slewEnableLimit.value())
            Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
        if (self.isInCrazyFlightmode == True):
            Config().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
        if (self.isInCrazyFlightmode == True):
            Config().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_pitch(value)
        Config().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_roll(value)
        Config().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(
            ("%0.2f %%" % self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
        logger.debug("Changed flightmode to %s",
                     self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(Config().get("normal_max_rp"))
            self.maxThrust.setValue(Config().get("normal_max_thrust"))
            self.minThrust.setValue(Config().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("normal_slew_rate"))
            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(Config().get("max_rp"))
            self.maxThrust.setValue(Config().get("max_thrust"))
            self.minThrust.setValue(Config().get("min_thrust"))
            self.slewEnableLimit.setValue(Config().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("slew_rate"))
            self.maxYawRate.setValue(Config().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.helper.cf.commander.set_client_xmode(checked)
        Config().set("client_side_xmode", checked)
        logger.info("Clientside X-mode enabled: %s", checked)

    def alt1_updated(self, state):
        if state:
            self._ring_effect += 1
            if self._ring_effect > self._ledring_nbr_effects:
                self._ring_effect = 0
            self.helper.cf.param.set_value("ring.effect",
                                           str(self._ring_effect))

    def alt2_updated(self, state):
        self.helper.cf.param.set_value("ring.headlightEnable", str(state))

    def _ring_populate_dropdown(self):
        nbr = int(self.helper.cf.param.values["ring"]["neffect"])
        current = int(self.helper.cf.param.values["ring"]["effect"])

        hardcoded_names = {
            0: "Off",
            1: "White spinner",
            2: "Color spinner",
            3: "Tilt effect",
            4: "Brightness effect",
            5: "Color spinner 2",
            6: "Double spinner",
            7: "Solid color effect",
            8: "Factory test",
            9: "Battery status",
            10: "Boat lights"
        }

        for i in range(nbr + 1):
            name = "{}: ".format(i)
            if i in hardcoded_names:
                name += hardcoded_names[i]
            else:
                name += "N/A"
            self._led_ring_effect.addItem(name, QVariant(i))

        self._led_ring_effect.setCurrentIndex(current)
        self._led_ring_effect.currentIndexChanged.connect(
            self._ring_effect_changed)
        self.helper.cf.param.add_update_callback(group="ring",
                                                 name="effect",
                                                 cb=self._ring_effect_updated)

    def _ring_effect_changed(self, index):
        i = self._led_ring_effect.itemData(index).toInt()[0]
        logger.info("Changed effect to {}".format(i))
        if i != self.helper.cf.param.values["ring"]["effect"]:
            self.helper.cf.param.set_value("ring.effect", str(i))

    def _ring_effect_updated(self, name, value):
        self._led_ring_effect.setCurrentIndex(int(value))
class FlightTab(Tab, flight_tab_class):

    uiSetupReadySignal = pyqtSignal()

    _mode_index = 0

    _motor_data_signal = pyqtSignal(int, object, object)
    _acc_data_signal = pyqtSignal(int, object, object)
    _imu_data_signal = pyqtSignal(int, object, object)
    _althold_data_signal = pyqtSignal(int, object, object)
    _baro_data_signal = pyqtSignal(int, object, object)
    _mag_data_signal = pyqtSignal(int, object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)
    _switch_mode_updated_signal = pyqtSignal()

    _log_error_signal = pyqtSignal(object, str)

    # UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.setStyleSheet(
            "QToolTip { color: #ffffff; background-color: #2d2d2d; border: 1px solid #f0f0f0; border-radius: 3px; }"
        )

        self.apitch = 0
        self.aroll = 0
        self.motor_power = 0
        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.ai = AttitudeIndicator()
        self.compass = CompassWidget()

        self.tabWidget = tabWidget
        self.helper = helper

        ########## Freefall related stuff
        self.ffd = FFD(parent=self)  # detection class
        self.ffr = FFR(parent=self)  # recovery class

        # Connect the crash/free fall detections
        self.ffd.sigFreeFall.connect(self.freefallDetected)
        self.ffd.sigCrashed.connect(self.crashDetected)
        self.ffr.sigRecoveryTimedOut.connect(self.recoveryAborted)

        # continuously send acc, mean, var to AI
        self.ffd.sigAccMeanVar.connect(self.ai.setFFAccMeanVar)
        # Only set recovery on if both fall and crash detection are on as well as recovery
        self.checkBox_ffd.stateChanged.connect(
            lambda on: self.ffGuiSync(0, on))  # detect freefall on/off
        self.checkBox_crash.stateChanged.connect(
            lambda on: self.ffGuiSync(1, on))  # detect crashing on/off
        self.checkBox_ffr.stateChanged.connect(
            lambda on: self.ffGuiSync(2, on))  # recovery crashing on/off

        # Use barometer for recovery (only clickedable if the checkbox activates in reaction to detecting a 10DOF flie
        self.checkBox_ffbaro.clicked.connect(self.ffr.setUseBaro)

        # intercept control commands
        self.helper.inputDeviceReader.auto_input_updated.add_callback(
            self.ffr.step)
        self.ffr.auto_input_updated.add_callback(
            self.helper.cf.commander.send_setpoint)
        self.ffr.auto_input_updated.add_callback(
            self._input_updated_signal.emit)
        self.ffr.althold_updated.add_callback(
            lambda param, arg: self.helper.cf.param.set_value(param, str(arg)))

        # Emergency Stop
        self._emergency_stop_updated_signal.connect(self.ffr.setKillSwitch)
        #self._emergency_stop_updated_signal.connect(self.ai.setKillSwitch)

        # Debugging Freefall
        self.pushButton_ff.clicked.connect(self.ffd.sendFakeEmit)
        self.pushButton_crash.clicked.connect(self.ffd.sendFakeLandingEmit)
        self.doubleSpinBox_ff_falloff.valueChanged.connect(
            self.ffr.falloff.setWidth)
        self.doubleSpinBox_ff_max.valueChanged.connect(
            self.ffr.falloff.setMaxThrust)
        self.doubleSpinBox_ff_min.valueChanged.connect(
            self.ffr.falloff.setMinThrust)
        self.doubleSpinBox_ff_time.valueChanged.connect(
            self.ffr.falloff.setTime)
        self.pushButton_plot.clicked.connect(self.ffr.falloff.plot)

        self.checkBox_debug.stateChanged.connect(self.toggleFFDebug)
        self.checkBox_ffbaro.clicked.connect(self.toggleFFDebug)

        # Slow down drawing to GUi items by keeping track of last received data time
        self.lastImuTime = 0

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
            self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
            self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
            self._emergency_stop_updated_signal.emit)

        self._switch_mode_updated_signal.connect(self.switchMode)
        self.helper.inputDeviceReader.switch_mode_updated.add_callback(
            self._switch_mode_updated_signal.emit)

        self.helper.inputDeviceReader.althold_updated.add_callback(
            self.changeHoldmode)

        self._imu_data_signal.connect(self._imu_data_received)
        self._baro_data_signal.connect(self._baro_data_received)
        self._althold_data_signal.connect(self._althold_data_received)
        self._motor_data_signal.connect(self._motor_data_received)
        self._acc_data_signal.connect(self._acc_data_received)
        self._mag_data_signal.connect(self._mag_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxAltitude.valueChanged.connect(self.maxAltitudeChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.clientHoldModeCheckbox.toggled.connect(self.changeHoldmode)
        self.isInCrazyFlightmode = False
        self.uiSetupReady()

        self.viscousModeSlider.valueChanged.connect(self.setViscousThrust)
        self.clientHoldModeCheckbox.setChecked(
            GuiConfig().get("client_side_holdmode"))

        # self.crazyflieXModeCheckbox.clicked.connect(
        #                     lambda enabled:
        #                     self.helper.cf.param.set_value("flightmode.x", str(enabled)))
        # self.helper.cf.param.add_update_callback(
        #                group="flightmode", name="xmode",
        #                cb=( lambda name, checked:
        #                self.crazyflieXModeCheckbox.setChecked(eval(checked))))
        self.ratePidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(enabled)))
        self.angularPidRadioButton.clicked.connect(
            lambda enabled: self.helper.cf.param.set_value(
                "flightmode.ratepid", str(not enabled)))
        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="ratepid",
            cb=(lambda name, checked: self.ratePidRadioButton.setChecked(
                eval(checked))))

        self.helper.cf.param.add_update_callback(
            group="flightmode",
            name="althold",
            cb=(lambda name, enabled: self.helper.inputDeviceReader.setAltHold(
                eval(enabled))))

        self.helper.cf.param.add_update_callback(
            group="imu_sensors", cb=self._set_available_sensors)

        self.logBaro = None
        self.logAltHold = None

        self.horizontalLayout_5.addWidget(self.ai)
        self.horizontalLayout_5.addWidget(self.compass)
        # self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000, 1])

        self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
        self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))

    def setViscousThrust(self, thrust):
        self.helper.inputDeviceReader.setViscousModeThrust(thrust)
        point = QtGui.QCursor.pos()
        QtGui.QToolTip.showText(
            QtCore.QPoint(point.x(),
                          self.viscousModeSlider.pos().y() * 2 - 12),
            QtCore.QString.number(thrust) + "%")

    @pyqtSlot(int)
    def toggleFFDebug(self, ignored=None):
        if self.checkBox_debug.isChecked():
            self.pushButton_ff.show()
            self.pushButton_crash.show()

            if self.checkBox_ffbaro.isEnabled(
            ) and self.checkBox_ffbaro.isChecked():
                self.pushButton_plot.hide()
                self.doubleSpinBox_ff_falloff.hide()
                self.doubleSpinBox_ff_max.hide()
                self.doubleSpinBox_ff_min.hide()
                self.doubleSpinBox_ff_time.hide()
                self.label_ff4.hide()
                self.label_ff1.hide()
                self.label_ff2.hide()
                self.label_ff3.hide()
            else:
                self.pushButton_plot.show()
                self.doubleSpinBox_ff_falloff.show()
                self.doubleSpinBox_ff_max.show()
                self.doubleSpinBox_ff_min.show()
                self.doubleSpinBox_ff_time.show()
                self.label_ff4.show()
                self.label_ff1.show()
                self.label_ff2.show()
                self.label_ff3.show()
        else:
            self.pushButton_ff.hide()
            self.pushButton_crash.hide()
            self.pushButton_plot.hide()
            self.doubleSpinBox_ff_falloff.hide()
            self.doubleSpinBox_ff_max.hide()
            self.doubleSpinBox_ff_min.hide()
            self.doubleSpinBox_ff_time.hide()
            self.label_ff4.hide()
            self.label_ff1.hide()
            self.label_ff2.hide()
            self.label_ff3.hide()

    @pyqtSlot()
    def freefallDetected(self):
        self.ai.setFreefall()
        if self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled():
            self.ffr.startRecovery()
            self.ai.setRecovery(True)
            self.helper.inputDeviceReader.setAuto(True)
            self.checkBox_ffr.setChecked(False)
            # self.emergency_stop_label.setText("Recovering Freefall")
        # self.checkBox_ffr.setEnabled(False)

    @pyqtSlot(float)
    def crashDetected(self, badness):
        self.ai.setRecovery(False, 'Landed / Crashed')
        self.ai.setCrash(badness)
        self.helper.inputDeviceReader.setAuto(False)
        self.ffr.setLanded()
        # self.emergency_stop_label.setText("")
        self.checkBox_ffr.setChecked(True)

    @pyqtSlot()
    def recoveryAborted(self):
        self.ai.setRecovery(False, 'Recovery Aborted / Timed out')
        self.helper.inputDeviceReader.setAuto(False)
        # self.emergency_stop_label.setText("")
        # self.checkBox_ffr.setEnabled(True)
        pass

    def ffGuiSync(self, id, on):
        """ Keeps freefall gui elements in sync"""
        if id == 0:  # freefall detection click
            self.ffd.setEnabled(on)  # detect freefall on/off
        elif id == 1:  # landing detection click
            self.ffd.setEnabledLanding(on)  # detect crashing on/off
        # elif id == 2: # recovery detection click

        # Only allow recivery checkbox to be turned on if crash/freefall detection is active
        self.checkBox_ffr.setEnabled(self.checkBox_ffd.isChecked()
                                     and self.checkBox_crash.isChecked())

        # Only allow recovery class to be turned on if all boxes are checked
        # self.ffr.setEnabled(on=self.checkBox_ffd.isChecked() and self.checkBox_crash.isChecked() and self.checkBox_ffr.isChecked() and self.checkBox_ffr.isEnabled())

    def makeSpace(self):
        # I wish I knew how to make the ai QWidget fullscreen....
        s = QtGui.QSplitter()
        if self.splitter_2.widget(0).isHidden():
            self.splitter_2.widget(0).show()
            self.splitter.widget(1).show()
        else:
            self.splitter_2.widget(0).hide()
            self.splitter.widget(1).hide()

    def _acc_data_received(self, timestamp, data, logconf):
        self.ffd.readAcc(data["acc.zw"])
        self.helper.cf.commander.setActualGravity(data)

    def updateCamList(self):
        """Repopulate the combobox with available camera devices and select the last one (=usually the camera we need)"""

        # Remove all
        self.comboBox_camera.clear()

        # Query available devices
        devices = self.cam.getDevices()
        for i, device in enumerate(devices):
            logger.debug("Camera device [%d]: %s", i, device)
            self.comboBox_camera.addItem(device)

        # enable/disable the on/off button and dropdown
        self.checkBox_camera.setEnabled(len(devices) > 0)
        self.comboBox_camera.setEnabled(len(devices) > 0)
        if len(devices) == 0:
            self.comboBox_camera.addItem("None Detected")

        # Select last one (eg the one we just plugged in)
        self.comboBox_camera.setCurrentIndex(max(0, len(devices) - 1))

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
            GuiConfig().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(
            self, "Log error", "Error when starting log config"
            " [%s]: %s" % (log_conf.name, msg))

    def _motor_data_received(self, timestamp, data, logconf):
        self.motor_power = data["motor.m1"] + data["motor.m2"] + data[
            "motor.m3"] + data["motor.m4"]
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])

    def _baro_data_received(self, timestamp, data, logconf):
        self.helper.inputDeviceReader.setCurrentAltitude(data["baro.aslLong"])
        if self.isVisible():
            self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
            self.ai.setBaro(data["baro.aslLong"])

    def _althold_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            target = data["altHold.target"]
            if target > 0:
                if not self.targetASL.isEnabled():
                    self.targetASL.setEnabled(True)
                self.targetASL.setText(("%.2f" % target))
                self.ai.setHover(target)
            elif self.targetASL.isEnabled():
                self.targetASL.setEnabled(False)
                self.targetASL.setText("Not set")
                self.ai.setHover(0)

    def _imu_data_received(self, timestamp, data, logconf):
        self.aroll = data["stabilizer.roll"]
        self.apitch = data["stabilizer.pitch"]
        if self.isVisible():
            self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
            self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
            self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
            self.actualThrust.setText(
                "%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))

            self.ai.setRollPitch(-data["stabilizer.roll"],
                                 data["stabilizer.pitch"])
            self.compass.setAngle(-data["stabilizer.yaw"])
            # self.helper.cf.commander.setActualPoint(data)

    def _mag_data_received(self, timestamp, data, logconf):
        # hard and soft correction values generated by Processing Magnetometer_calibration script + calibrate_hardsoft.py
        magn_ellipsoid_center = [1341.66, -537.690, 41.1584]
        magn_ellipsoid_transform = [[0.934687, 0.0222809, 0.0151145],
                                    [0.0222809, 0.919365, -0.00622367],
                                    [0.0151145, -0.00622367, 0.996487]]

        # values generated by calibrate_powered.py
        qx = [
            0.067946222436498283, -0.25034004667098259, 8.3336994198409666,
            -0.17762637163222378
        ]
        qy = [
            -0.13945102271766135, 2.9074808469097495, 1.6764850422889934,
            0.19244505046927501
        ]
        qz = [
            0.018800599305554239, -0.79590273035713055, -3.1033531112103478,
            0.13550993988096199
        ]

        # matrix by vector multiplication
        def mv(a, b):
            out = [0, 0, 0]
            for x in range(0, 3):
                out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2]
            return out

        # calculate adjustments related to how much power is sent to the motors
        def adj(qs, power):
            p = float(power) / float(40000)  # 10k * 4 motors
            return qs[0] * p**3 + qs[1] * p**2 + qs[2] * p + qs[3]

        x, y, z = data['mag.x'], data['mag.y'], data['mag.z']

        x = x - magn_ellipsoid_center[0]
        y = y - magn_ellipsoid_center[1]
        z = z - magn_ellipsoid_center[2]
        x, y, z = mv(magn_ellipsoid_transform, [x, y, z])

        x = x + adj(qx, self.motor_power)
        y = y + adj(qy, self.motor_power)
        z = z + adj(qz, self.motor_power)

        # correct magnetometer orientation relative to the CF orientation
        x, y, z = y, x, z * -1

        # calculate tilt-compensated heading angle
        cosRoll = cos(math.radians(self.aroll))
        sinRoll = sin(math.radians(self.aroll))
        cosPitch = cos(math.radians(self.apitch))
        sinPitch = sin(math.radians(self.apitch))

        Xh = x * cosPitch + z * sinPitch
        Yh = x * sinRoll * sinPitch + y * cosRoll - z * sinRoll * cosPitch

        heading = math.atan2(Yh, Xh)
        d_heading = math.degrees(
            heading) * -1  # for some reason getting inveted sign here

        # update compass widget
        #self.compass.setAngle(d_heading)

        # lock heading to 0 degrees north
        if d_heading > 0:
            yaw_trim = -20
        else:
            yaw_trim = 20
        self.helper.inputDeviceReader.update_trim_yaw_signal(yaw_trim)

    def connected(self, linkURI):
        # IMU & THRUST
        lg = LogConfig("Stabalizer", GuiConfig().get("ui_update_period"))
        lg.add_variable("stabilizer.roll", "float")
        lg.add_variable("stabilizer.pitch", "float")
        lg.add_variable("stabilizer.yaw", "float")
        lg.add_variable("stabilizer.thrust", "uint16_t")
        lg.add_variable("acc.x", "float")
        lg.add_variable("acc.y", "float")
        lg.add_variable("acc.z", "float")

        self.helper.cf.log.add_config(lg)
        if (lg.valid):
            lg.data_received_cb.add_callback(self._imu_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")

        # MOTOR
        lg = LogConfig("Motors", GuiConfig().get("ui_update_period"))
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._motor_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning("Could not setup logconfiguration after "
                           "connection!")

        # Acc for freefall
        lg = LogConfig(
            "Acc", 10)  # Yes we need this at 100hz for freefall detection!!
        lg.add_variable("acc.zw", "float")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._acc_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning(
                "Could not setup logconfiguration after connection [ACC]!")

        lg = LogConfig("Magnetometer", 100)
        lg.add_variable("mag.x", "int16_t")
        lg.add_variable("mag.y", "int16_t")
        lg.add_variable("mag.z", "int16_t")

        self.helper.cf.log.add_config(lg)
        if lg.valid:
            lg.data_received_cb.add_callback(self._mag_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        else:
            logger.warning(
                "Could not setup logconfiguration after connection!")

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)
        if ("HMC5883L" in name):
            if (not available):
                self.maxAltitudeLabel.setEnabled(False)
                self.maxAltitude.setEnabled(False)
                self.actualASL.setText("N/A")
                self.actualASL.setEnabled(False)
                self.checkBox_ffbaro.setEnabled(False)
                self.checkBox_ffbaro.setChecked(False)
            else:
                self.actualASL.setEnabled(True)
                self.checkBox_ffbaro.setEnabled(True)
                self.helper.inputDeviceReader.setAltHoldAvailable(available)
                if (not self.logBaro and not self.logAltHold):
                    # The sensor is available, set up the logging
                    self.logBaro = LogConfig("Baro", 200)
                    self.logBaro.add_variable("baro.aslLong", "float")

                    self.helper.cf.log.add_config(self.logBaro)
                    if self.logBaro.valid:
                        self.logBaro.data_received_cb.add_callback(
                            self._baro_data_signal.emit)
                        self.logBaro.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logBaro.start()
                    else:
                        logger.warning(
                            "Could not setup logconfiguration after "
                            "connection!")
                    self.logAltHold = LogConfig("AltHold", 200)
                    self.logAltHold.add_variable("altHold.target", "float")

                    self.helper.cf.log.add_config(self.logAltHold)
                    if self.logAltHold.valid:
                        self.logAltHold.data_received_cb.add_callback(
                            self._althold_data_signal.emit)
                        self.logAltHold.error_cb.add_callback(
                            self._log_error_signal.emit)
                        self.logAltHold.start()
                    else:
                        logger.warning(
                            "Could not setup logconfiguration after "
                            "connection!")

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)
        self.actualRoll.setText("")
        self.actualPitch.setText("")
        self.actualYaw.setText("")
        self.actualThrust.setText("")
        self.actualASL.setText("")
        self.targetASL.setText("Not Set")
        self.targetASL.setEnabled(False)
        self.actualASL.setEnabled(False)
        self.logBaro = None
        self.logAltHold = None

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.set_thrust_limits(
            self.minThrust.value(), self.maxThrust.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("min_thrust", self.minThrust.value())
            GuiConfig().set("max_thrust", self.maxThrust.value())

    def maxAltitudeChanged(self):
        self.helper.inputDeviceReader.setMaxAltitude(self.maxAltitude.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.set_thrust_slew_limiting(
            self.thrustLoweringSlewRateLimit.value(),
            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("slew_limit", self.slewEnableLimit.value())
            GuiConfig().set("slew_rate",
                            self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
        if (self.isInCrazyFlightmode == True):
            GuiConfig().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_pitch(value)
        GuiConfig().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.set_trim_roll(value)
        GuiConfig().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f" % roll))
        self.targetPitch.setText(("%0.2f" % pitch))
        self.targetYaw.setText(("%0.2f" % yaw))
        self.targetThrust.setText(
            ("%0.2f %%" % self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setEnabled(True)
            self.emergency_stop_label.setText(
                self.emergencyStopStringWithText("Kill Switch Active"))
            self.ai.setKillSwitch(True)
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("Kill Swith")
            self.emergency_stop_label.setEnabled(False)
            self.ai.setKillSwitch(False)

    def flightmodeChange(self, item):
        GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
        logger.info("Changed flightmode to %s",
                    self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
            self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
            self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                GuiConfig().get("normal_slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(GuiConfig().get("max_rp"))
            self.maxThrust.setValue(GuiConfig().get("max_thrust"))
            self.minThrust.setValue(GuiConfig().get("min_thrust"))
            self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                GuiConfig().get("slew_rate"))
            self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    @pyqtSlot(bool)
    def changeCareFreemode(self, checked):
        # self.helper.cf.commander.set_client_carefreemode(checked)
        if checked:
            self.clientCareFreeModeRadio.setChecked(checked)
            self.helper.cf.param.set_value("FlightMode.flightmode", "0")
            # self.helper.cf.commander.set_client_positionmode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        elif not checked and self._mode_index == 1:
            self.clientCareFreeModeRadio.setChecked(False)
            self.clientNormalModeRadio.setChecked(True)
            self.helper.cf.param.set_value("FlightMode.flightmode", "1")
        logger.info("CareFree-mode enabled: %s", checked)

    @pyqtSlot(bool)
    def changeXmode(self, checked):
        self.clientXModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_xmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "2")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_positionmode(False)
        logger.info("X-mode enabled: %s", checked)

    @pyqtSlot(bool)
    def changePositionmode(self, checked):
        self.clientPositionModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_positionmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "3")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        logger.info("Position-mode enabled: %s", checked)

    @pyqtSlot(bool)
    def changeHeadingmode(self, checked):
        self.clientHeadingModeRadio.setChecked(checked)
        # self.helper.cf.commander.set_client_positionmode(checked)
        if checked:
            self.helper.cf.param.set_value("FlightMode.flightmode", "4")
            # self.helper.cf.commander.set_client_carefreemode(False)
            # self.helper.cf.commander.set_client_xmode(False)
        logger.info("Heading-mode enabled: %s", checked)

    @pyqtSlot(bool)
    def changeHoldmode(self, checked):
        self.clientHoldModeCheckbox.setChecked(checked)
        self.helper.cf.param.set_value("flightmode.althold", str(checked))
        # self.helper.cf.commander.set_client_holdmode(checked)
        GuiConfig().set("client_side_holdmode", checked)
        logger.info("Hold-mode enabled: %s", checked)

    def switchMode(self):
        if self._mode_index == 4:
            self._mode_index = 0
        else:
            self._mode_index += 1
        if self._mode_index == 0:
            self.changeCareFreemode(True)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 1:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 2:
            self.changeCareFreemode(False)
            self.changeXmode(True)
            self.changePositionmode(False)
            self.changeHeadingmode(False)
        elif self._mode_index == 3:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(True)
            self.changeHeadingmode(False)
        elif self._mode_index == 4:
            self.changeCareFreemode(False)
            self.changeXmode(False)
            self.changePositionmode(False)
            self.changeHeadingmode(True)
        logger.info("Mode switched to index: %d", self._mode_index)
예제 #9
0
class FlightTab(Tab, flight_tab_class):
    uiSetupReadySignal = pyqtSignal()

    _log_data_signal = pyqtSignal(int, object, object)
    _pose_data_signal = pyqtSignal(object, object)

    _input_updated_signal = pyqtSignal(float, float, float, float)
    _rp_trim_updated_signal = pyqtSignal(float, float)
    _emergency_stop_updated_signal = pyqtSignal(bool)
    _assisted_control_updated_signal = pyqtSignal(bool)
    _heighthold_input_updated_signal = pyqtSignal(float, float, float, float)
    _hover_input_updated_signal = pyqtSignal(float, float, float, float)

    _log_error_signal = pyqtSignal(object, str)

    # UI_DATA_UPDATE_FPS = 10

    connectionFinishedSignal = pyqtSignal(str)
    disconnectedSignal = pyqtSignal(str)

    _limiting_updated = pyqtSignal(bool, bool, bool)

    def __init__(self, tabWidget, helper, *args):
        super(FlightTab, self).__init__(*args)
        self.setupUi(self)

        self.tabName = "Flight Control"
        self.menuName = "Flight Control"

        self.tabWidget = tabWidget
        self.helper = helper

        self.disconnectedSignal.connect(self.disconnected)
        self.connectionFinishedSignal.connect(self.connected)
        # Incomming signals
        self.helper.cf.connected.add_callback(
            self.connectionFinishedSignal.emit)
        self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)

        self._input_updated_signal.connect(self.updateInputControl)
        self.helper.inputDeviceReader.input_updated.add_callback(
            self._input_updated_signal.emit)
        self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
        self.helper.inputDeviceReader.rp_trim_updated.add_callback(
            self._rp_trim_updated_signal.emit)
        self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
        self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
            self._emergency_stop_updated_signal.emit)

        self.helper.inputDeviceReader.heighthold_input_updated.add_callback(
            self._heighthold_input_updated_signal.emit)
        self._heighthold_input_updated_signal.connect(
            self._heighthold_input_updated)
        self.helper.inputDeviceReader.hover_input_updated.add_callback(
            self._hover_input_updated_signal.emit)
        self._hover_input_updated_signal.connect(
            self._hover_input_updated)

        self.helper.inputDeviceReader.assisted_control_updated.add_callback(
            self._assisted_control_updated_signal.emit)

        self._assisted_control_updated_signal.connect(
            self._assisted_control_updated)

        self._pose_data_signal.connect(self._pose_data_received)
        self._log_data_signal.connect(self._log_data_received)

        self._log_error_signal.connect(self._logging_error)

        # Connect UI signals that are in this tab
        self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
        self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
        self.thrustLoweringSlewRateLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.slewEnableLimit.valueChanged.connect(
            self.thrustLoweringSlewRateLimitChanged)
        self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
        self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
        self.maxAngle.valueChanged.connect(self.maxAngleChanged)
        self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
        self.uiSetupReadySignal.connect(self.uiSetupReady)
        self.isInCrazyFlightmode = False

        # Command Based Flight Control
        self._can_fly = 0
        self._hlCommander = None
        self.commanderTakeOffButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.TAKE_OFF)
        )
        self.commanderLandButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.LAND)
        )
        self.commanderLeftButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.LEFT)
        )
        self.commanderRightButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.RIGHT)
        )
        self.commanderForwardButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.FORWARD)
        )
        self.commanderBackButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.BACK)
        )
        self.commanderUpButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.UP)
        )
        self.commanderDownButton.clicked.connect(
            lambda: self._flight_command(CommanderAction.DOWN)
        )

        self.uiSetupReady()

        self.ratePidRadioButton.clicked.connect(
            lambda enabled:
            self.helper.cf.param.set_value("flightmode.ratepid",
                                           str(enabled)))

        self.angularPidRadioButton.clicked.connect(
            lambda enabled:
            self.helper.cf.param.set_value("flightmode.ratepid",
                                           str(not enabled)))

        self._led_ring_headlight.clicked.connect(
            lambda enabled:
            self.helper.cf.param.set_value("ring.headlightEnable",
                                           str(enabled)))

        self.helper.cf.param.add_update_callback(
            group="flightmode", name="ratepid",
            cb=(lambda name, checked:
                self.ratePidRadioButton.setChecked(eval(checked))))

        self.helper.cf.param.add_update_callback(
            group="ring", name="headlightEnable",
            cb=(lambda name, checked:
                self._led_ring_headlight.setChecked(eval(checked))))

        self._ledring_nbr_effects = 0

        self.helper.cf.param.add_update_callback(
            group="ring",
            name="effect",
            cb=self._ring_effect_updated)

        self.helper.cf.param.add_update_callback(
            group="imu_sensors",
            cb=self._set_available_sensors)

        self.helper.cf.param.all_updated.add_callback(
            self._all_params_updated)

        self.logAltHold = None

        self.ai = AttitudeIndicator()
        self.verticalLayout_4.addWidget(self.ai)
        self.splitter.setSizes([1000, 1])

        self.targetCalPitch.setValue(Config().get("trim_pitch"))
        self.targetCalRoll.setValue(Config().get("trim_roll"))

        self.helper.inputDeviceReader.alt1_updated.add_callback(
            self.alt1_updated)
        self.helper.inputDeviceReader.alt2_updated.add_callback(
            self.alt2_updated)
        self._tf_state = 0
        self._ring_effect = 0

        # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
        self.helper.inputDeviceReader.limiting_updated.add_callback(
            self._limiting_updated.emit)
        self._limiting_updated.connect(self._set_limiting_enabled)

        self.helper.pose_logger.data_received_cb.add_callback(
            self._pose_data_signal.emit)

    def _set_limiting_enabled(self, rp_limiting_enabled,
                              yaw_limiting_enabled,
                              thrust_limiting_enabled):
        self.maxAngle.setEnabled(rp_limiting_enabled)
        self.targetCalRoll.setEnabled(rp_limiting_enabled)
        self.targetCalPitch.setEnabled(rp_limiting_enabled)
        self.maxYawRate.setEnabled(yaw_limiting_enabled)
        self.maxThrust.setEnabled(thrust_limiting_enabled)
        self.minThrust.setEnabled(thrust_limiting_enabled)
        self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
        self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)

    def thrustToPercentage(self, thrust):
        return ((thrust / MAX_THRUST) * 100.0)

    def uiSetupReady(self):
        flightComboIndex = self.flightModeCombo.findText(
            Config().get("flightmode"), Qt.MatchFixedString)
        if (flightComboIndex < 0):
            self.flightModeCombo.setCurrentIndex(0)
            self.flightModeCombo.currentIndexChanged.emit(0)
        else:
            self.flightModeCombo.setCurrentIndex(flightComboIndex)
            self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

    def _flight_command(self, action):
        if self._hlCommander is None:
            return

        if action == CommanderAction.TAKE_OFF:
            #
            # Reset the Kalman filter before taking off, to avoid
            # positional confusion.
            #
            self.helper.cf.param.set_value('kalman.resetEstimation', '1')
            time.sleep(1)
            self._hlCommander.take_off()
        elif action == CommanderAction.LAND:
            self._hlCommander.land()
        elif action == CommanderAction.LEFT:
            self._hlCommander.left(0.5)
        elif action == CommanderAction.RIGHT:
            self._hlCommander.right(0.5)
        elif action == CommanderAction.FORWARD:
            self._hlCommander.forward(0.5)
        elif action == CommanderAction.BACK:
            self._hlCommander.back(0.5)
        elif action == CommanderAction.UP:
            self._hlCommander.up(0.5)
        elif action == CommanderAction.DOWN:
            self._hlCommander.down(0.5)

    def _logging_error(self, log_conf, msg):
        QMessageBox.about(self, "Log error",
                          "Error when starting log config [%s]: %s" % (
                              log_conf.name, msg))

    def _log_data_received(self, timestamp, data, logconf):
        if self.isVisible():
            self.actualM1.setValue(data["motor.m1"])
            self.actualM2.setValue(data["motor.m2"])
            self.actualM3.setValue(data["motor.m3"])
            self.actualM4.setValue(data["motor.m4"])

            self.estimateThrust.setText(
                "%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))

            if data["sys.canfly"] != self._can_fly:
                self._can_fly = data["sys.canfly"]
                self._update_flight_commander(True)

    def _pose_data_received(self, pose_logger, pose):
        if self.isVisible():
            estimated_z = pose[2]
            roll = pose[3]
            pitch = pose[4]

            self.estimateX.setText(("%.2f" % pose[0]))
            self.estimateY.setText(("%.2f" % pose[1]))
            self.estimateZ.setText(("%.2f" % estimated_z))
            self.estimateRoll.setText(("%.2f" % roll))
            self.estimatePitch.setText(("%.2f" % pitch))
            self.estimateYaw.setText(("%.2f" % pose[5]))

            self.ai.setBaro(estimated_z, self.is_visible())
            self.ai.setRollPitch(-roll, pitch, self.is_visible())

    def _heighthold_input_updated(self, roll, pitch, yaw, height):
        if (self.isVisible() and
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 self.helper.inputDeviceReader.ASSISTED_CONTROL_HEIGHTHOLD)):

            self.targetRoll.setText(("%0.2f deg" % roll))
            self.targetPitch.setText(("%0.2f deg" % pitch))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

            self._change_input_labels(using_hover_assist=False)

    def _hover_input_updated(self, vx, vy, yaw, height):
        if (self.isVisible() and
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 self.helper.inputDeviceReader.ASSISTED_CONTROL_HOVER)):

            self.targetRoll.setText(("%0.2f m/s" % vy))
            self.targetPitch.setText(("%0.2f m/s" % vx))
            self.targetYaw.setText(("%0.2f deg/s" % yaw))
            self.targetHeight.setText(("%.2f m" % height))
            self.ai.setHover(height, self.is_visible())

            self._change_input_labels(using_hover_assist=True)

    def _change_input_labels(self, using_hover_assist):
        if using_hover_assist:
            pitch, roll, yaw = 'Velocity X', 'Velocity Y', 'Velocity Z'
        else:
            pitch, roll, yaw = 'Pitch', 'Roll', 'Yaw'

        self.inputPitchLabel.setText(pitch)
        self.inputRollLabel.setText(roll)
        self.inputYawLabel.setText(yaw)

    def _update_flight_commander(self, connected):
        self.commanderBox.setToolTip(str())
        if not connected:
            self.commanderBox.setEnabled(False)
            return

        if self._can_fly == 0:
            self.commanderBox.setEnabled(False)
            self.commanderBox.setToolTip(
                'The Crazyflie reports that flight is not possible'
            )
            return

        #                  flowV1    flowV2     LightHouse       LPS
        position_decks = ['bcFlow', 'bcFlow2', 'bcLighthouse4', 'bcDWM1000']
        for deck in position_decks:
            if int(self.helper.cf.param.values['deck'][deck]) == 1:
                self.commanderBox.setEnabled(True)
                break
        else:
            self.commanderBox.setToolTip(
                'You need a positioning deck to use Command Based Flight'
            )
            self.commanderBox.setEnabled(False)

    def connected(self, linkURI):
        # MOTOR & THRUST
        lg = LogConfig("Motors", Config().get("ui_update_period"))
        lg.add_variable("stabilizer.thrust", "uint16_t")
        lg.add_variable("motor.m1")
        lg.add_variable("motor.m2")
        lg.add_variable("motor.m3")
        lg.add_variable("motor.m4")
        lg.add_variable("sys.canfly")

        self._hlCommander = PositionHlCommander(
            self.helper.cf,
            x=0.0, y=0.0, z=0.0,
            default_velocity=0.3,
            default_height=0.5,
            controller=PositionHlCommander.CONTROLLER_PID
        )

        try:
            self.helper.cf.log.add_config(lg)
            lg.data_received_cb.add_callback(self._log_data_signal.emit)
            lg.error_cb.add_callback(self._log_error_signal.emit)
            lg.start()
        except KeyError as e:
            logger.warning(str(e))
        except AttributeError as e:
            logger.warning(str(e))

    def _enable_estimators(self, should_enable):
        self.estimateX.setEnabled(should_enable)
        self.estimateY.setEnabled(should_enable)
        self.estimateZ.setEnabled(should_enable)

    def _set_available_sensors(self, name, available):
        logger.info("[%s]: %s", name, available)
        available = eval(available)

        self._enable_estimators(True)
        self.helper.inputDeviceReader.set_alt_hold_available(available)

    def disconnected(self, linkURI):
        self.ai.setRollPitch(0, 0)
        self.actualM1.setValue(0)
        self.actualM2.setValue(0)
        self.actualM3.setValue(0)
        self.actualM4.setValue(0)

        self.estimateRoll.setText("")
        self.estimatePitch.setText("")
        self.estimateYaw.setText("")
        self.estimateThrust.setText("")
        self.estimateX.setText("")
        self.estimateY.setText("")
        self.estimateZ.setText("")

        self.targetHeight.setText("Not Set")
        self.ai.setHover(0, self.is_visible())
        self.targetHeight.setEnabled(False)

        self._enable_estimators(False)

        self.logAltHold = None
        self._led_ring_effect.setEnabled(False)
        self._led_ring_effect.clear()
        try:
            self._led_ring_effect.currentIndexChanged.disconnect(
                self._ring_effect_changed)
        except TypeError:
            # Signal was not connected
            pass
        self._led_ring_effect.setCurrentIndex(-1)
        self._led_ring_headlight.setEnabled(False)

        try:
            self._assist_mode_combo.currentIndexChanged.disconnect(
                self._assist_mode_changed)
        except TypeError:
            # Signal was not connected
            pass
        self._assist_mode_combo.setEnabled(False)
        self._assist_mode_combo.clear()

        self._update_flight_commander(False)

    def minMaxThrustChanged(self):
        self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
        self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("min_thrust", self.minThrust.value())
            Config().set("max_thrust", self.maxThrust.value())

    def thrustLoweringSlewRateLimitChanged(self):
        self.helper.inputDeviceReader.thrust_slew_rate = (
            self.thrustLoweringSlewRateLimit.value())
        self.helper.inputDeviceReader.thrust_slew_limit = (
            self.slewEnableLimit.value())
        if (self.isInCrazyFlightmode is True):
            Config().set("slew_limit", self.slewEnableLimit.value())
            Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())

    def maxYawRateChanged(self):
        logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
        self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_yaw", self.maxYawRate.value())

    def maxAngleChanged(self):
        logger.debug("MaxAngle changed to %d", self.maxAngle.value())
        self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
        if (self.isInCrazyFlightmode is True):
            Config().set("max_rp", self.maxAngle.value())

    def _trim_pitch_changed(self, value):
        logger.debug("Pitch trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_pitch = value
        Config().set("trim_pitch", value)

    def _trim_roll_changed(self, value):
        logger.debug("Roll trim updated to [%f]" % value)
        self.helper.inputDeviceReader.trim_roll = value
        Config().set("trim_roll", value)

    def calUpdateFromInput(self, rollCal, pitchCal):
        logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
                     rollCal, pitchCal)
        self.targetCalRoll.setValue(rollCal)
        self.targetCalPitch.setValue(pitchCal)

    def updateInputControl(self, roll, pitch, yaw, thrust):
        self.targetRoll.setText(("%0.2f deg" % roll))
        self.targetPitch.setText(("%0.2f deg" % pitch))
        self.targetYaw.setText(("%0.2f deg/s" % yaw))
        self.targetThrust.setText(("%0.2f %%" %
                                   self.thrustToPercentage(thrust)))
        self.thrustProgress.setValue(thrust)

        self._change_input_labels(using_hover_assist=False)

    def setMotorLabelsEnabled(self, enabled):
        self.M1label.setEnabled(enabled)
        self.M2label.setEnabled(enabled)
        self.M3label.setEnabled(enabled)
        self.M4label.setEnabled(enabled)

    def emergencyStopStringWithText(self, text):
        return ("<html><head/><body><p>"
                "<span style='font-weight:600; color:#7b0005;'>{}</span>"
                "</p></body></html>".format(text))

    def updateEmergencyStop(self, emergencyStop):
        if emergencyStop:
            self.setMotorLabelsEnabled(False)
            self.emergency_stop_label.setText(
                self.emergencyStopStringWithText("Kill switch active"))
        else:
            self.setMotorLabelsEnabled(True)
            self.emergency_stop_label.setText("")

    def flightmodeChange(self, item):
        Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
        logger.debug("Changed flightmode to %s",
                     self.flightModeCombo.itemText(item))
        self.isInCrazyFlightmode = False
        if (item == 0):  # Normal
            self.maxAngle.setValue(Config().get("normal_max_rp"))
            self.maxThrust.setValue(Config().get("normal_max_thrust"))
            self.minThrust.setValue(Config().get("normal_min_thrust"))
            self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("normal_slew_rate"))
            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
        if (item == 1):  # Advanced
            self.maxAngle.setValue(Config().get("max_rp"))
            self.maxThrust.setValue(Config().get("max_thrust"))
            self.minThrust.setValue(Config().get("min_thrust"))
            self.slewEnableLimit.setValue(Config().get("slew_limit"))
            self.thrustLoweringSlewRateLimit.setValue(
                Config().get("slew_rate"))
            self.maxYawRate.setValue(Config().get("max_yaw"))
            self.isInCrazyFlightmode = True

        if (item == 0):
            newState = False
        else:
            newState = True
        self.maxThrust.setEnabled(newState)
        self.maxAngle.setEnabled(newState)
        self.minThrust.setEnabled(newState)
        self.thrustLoweringSlewRateLimit.setEnabled(newState)
        self.slewEnableLimit.setEnabled(newState)
        self.maxYawRate.setEnabled(newState)

    def _assist_mode_changed(self, item):
        mode = None

        if (item == 0):  # Altitude hold
            mode = JoystickReader.ASSISTED_CONTROL_ALTHOLD
        if (item == 1):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_POSHOLD
        if (item == 2):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD
        if (item == 3):  # Position hold
            mode = JoystickReader.ASSISTED_CONTROL_HOVER

        self.helper.inputDeviceReader.set_assisted_control(mode)
        Config().set("assistedControl", mode)

    def _assisted_control_updated(self, enabled):
        if self.helper.inputDeviceReader.get_assisted_control() == \
                JoystickReader.ASSISTED_CONTROL_POSHOLD:
            self.targetThrust.setEnabled(not enabled)
            self.targetRoll.setEnabled(not enabled)
            self.targetPitch.setEnabled(not enabled)
        elif ((self.helper.inputDeviceReader.get_assisted_control() ==
                JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD) or
                (self.helper.inputDeviceReader.get_assisted_control() ==
                 JoystickReader.ASSISTED_CONTROL_HOVER)):
            self.targetThrust.setEnabled(not enabled)
            self.targetHeight.setEnabled(enabled)
            print('Chaning enable for target height: %s' % enabled)
        else:
            self.helper.cf.param.set_value("flightmode.althold", str(enabled))

    def alt1_updated(self, state):
        if state:
            new_index = (self._ring_effect+1) % (self._ledring_nbr_effects+1)
            self.helper.cf.param.set_value("ring.effect",
                                           str(new_index))

    def alt2_updated(self, state):
        self.helper.cf.param.set_value("ring.headlightEnable", str(state))

    def _all_params_updated(self):
        self._ring_populate_dropdown()
        self._populate_assisted_mode_dropdown()
        self._update_flight_commander(True)

    def _ring_populate_dropdown(self):
        try:
            nbr = int(self.helper.cf.param.values["ring"]["neffect"])
            current = int(self.helper.cf.param.values["ring"]["effect"])
        except KeyError:
            return

        # Used only in alt1_updated function
        self._ring_effect = current
        self._ledring_nbr_effects = nbr

        hardcoded_names = {
            0: "Off",
            1: "White spinner",
            2: "Color spinner",
            3: "Tilt effect",
            4: "Brightness effect",
            5: "Color spinner 2",
            6: "Double spinner",
            7: "Solid color effect",
            8: "Factory test",
            9: "Battery status",
            10: "Boat lights",
            11: "Alert",
            12: "Gravity",
            13: "LED tab",
            14: "Color fader",
            15: "Link quality",
            16: "Location server status",
            17: "Sequencer",
            18: "Lighthouse quality",
        }

        for i in range(nbr + 1):
            name = "{}: ".format(i)
            if i in hardcoded_names:
                name += hardcoded_names[i]
            else:
                name += "N/A"
            self._led_ring_effect.addItem(name, i)

        self._led_ring_effect.currentIndexChanged.connect(
            self._ring_effect_changed)

        self._led_ring_effect.setCurrentIndex(current)
        if int(self.helper.cf.param.values["deck"]["bcLedRing"]) == 1:
            self._led_ring_effect.setEnabled(True)
            self._led_ring_headlight.setEnabled(True)

    def _ring_effect_changed(self, index):
        self._ring_effect = index
        if index > -1:
            i = self._led_ring_effect.itemData(index)
            logger.info("Changed effect to {}".format(i))
            if i != int(self.helper.cf.param.values["ring"]["effect"]):
                self.helper.cf.param.set_value("ring.effect", str(i))

    def _ring_effect_updated(self, name, value):
        if self.helper.cf.param.is_updated:
            self._led_ring_effect.setCurrentIndex(int(value))

    def _populate_assisted_mode_dropdown(self):
        self._assist_mode_combo.addItem("Altitude hold", 0)
        self._assist_mode_combo.addItem("Position hold", 1)
        self._assist_mode_combo.addItem("Height hold", 2)
        self._assist_mode_combo.addItem("Hover", 3)

        # Add the tooltips to the assist-mode items.
        self._assist_mode_combo.setItemData(0, TOOLTIP_ALTITUDE_HOLD,
                                            Qt.ToolTipRole)
        self._assist_mode_combo.setItemData(1, TOOLTIP_POSITION_HOLD,
                                            Qt.ToolTipRole)
        self._assist_mode_combo.setItemData(2, TOOLTIP_HEIGHT_HOLD,
                                            Qt.ToolTipRole)
        self._assist_mode_combo.setItemData(3, TOOLTIP_HOVER,
                                            Qt.ToolTipRole)

        heightHoldPossible = False
        hoverPossible = False

        if int(self.helper.cf.param.values["deck"]["bcZRanger"]) == 1:
            heightHoldPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(1.0)

        if int(self.helper.cf.param.values["deck"]["bcZRanger2"]) == 1:
            heightHoldPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(2.0)

        if int(self.helper.cf.param.values["deck"]["bcFlow"]) == 1:
            heightHoldPossible = True
            hoverPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(1.0)

        if int(self.helper.cf.param.values["deck"]["bcFlow2"]) == 1:
            heightHoldPossible = True
            hoverPossible = True
            self.helper.inputDeviceReader.set_hover_max_height(2.0)

        if not heightHoldPossible:
            self._assist_mode_combo.model().item(2).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        if not hoverPossible:
            self._assist_mode_combo.model().item(3).setEnabled(False)
        else:
            self._assist_mode_combo.model().item(0).setEnabled(False)

        self._assist_mode_combo.currentIndexChanged.connect(
            self._assist_mode_changed)
        self._assist_mode_combo.setEnabled(True)

        try:
            assistmodeComboIndex = Config().get("assistedControl")
            if assistmodeComboIndex == 3 and not hoverPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and hoverPossible:
                self._assist_mode_combo.setCurrentIndex(3)
                self._assist_mode_combo.currentIndexChanged.emit(3)
            elif assistmodeComboIndex == 2 and not heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(0)
                self._assist_mode_combo.currentIndexChanged.emit(0)
            elif assistmodeComboIndex == 0 and heightHoldPossible:
                self._assist_mode_combo.setCurrentIndex(2)
                self._assist_mode_combo.currentIndexChanged.emit(2)
            else:
                self._assist_mode_combo.setCurrentIndex(assistmodeComboIndex)
                self._assist_mode_combo.currentIndexChanged.emit(
                                                    assistmodeComboIndex)
        except KeyError:
            defaultOption = 0
            if hoverPossible:
                defaultOption = 3
            elif heightHoldPossible:
                defaultOption = 2
            self._assist_mode_combo.setCurrentIndex(defaultOption)
            self._assist_mode_combo.currentIndexChanged.emit(defaultOption)