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