def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=True)
for d in devs:
node = QAction(d["name"], self._menu_devices, checkable=True)
node.toggled.connect(self._inputdevice_selected)
group.addAction(node)
self._menu_devices.addAction(node)
if (d["name"] == GuiConfig().get("input_device")):
self._active_device = d["name"]
if (len(self._active_device) == 0):
self._active_device = self._menu_devices.actions()[0].text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (device_config_mapping):
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[str(
self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions(
)[0].text()
else:
self._current_input_config = self._menu_mappings.actions()[0].text(
)
# Now we know what device to use and what mapping, trigger the events
# to change the menus and start the input
for c in self._menu_mappings.actions():
c.setEnabled(True)
if (c.text() == self._current_input_config):
c.setChecked(True)
for c in self._menu_devices.actions():
if (c.text() == self._active_device):
c.setChecked(True)
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.set_thrust_limits(
self.percentageToThrust(self.minThrust.value()),
self.percentageToThrust(self.maxThrust.value()))
if (self.isInCrazyFlightmode == True):
GuiConfig().set("min_thrust", self.minThrust.value())
GuiConfig().set("max_thrust", self.maxThrust.value())
def toggleVisibility(self, checked):
"""Show or hide the tab."""
if checked:
self.tabWidget.addTab(self, self.getTabName())
s = ""
try:
s = GuiConfig().get("open_tabs")
if (len(s) > 0):
s += ","
except Exception as e:
logger.warning("Exception while adding tab to config and "
"reading tab config")
# Check this since tabs in config are opened when app is started
if (self.tabName not in s):
s += "%s" % self.tabName
GuiConfig().set("open_tabs", s)
if not checked:
self.tabWidget.removeTab(self.tabWidget.indexOf(self))
try:
parts = GuiConfig().get("open_tabs").split(",")
except Exception as e:
logger.warning("Exception while removing tab from config and "
"reading tab config")
parts = []
s = ""
for p in parts:
if (self.tabName != p):
s += "%s," % p
s = s[0:len(s) - 1] # Remove last comma
GuiConfig().set("open_tabs", s)
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!")
def _update_input(self, device="", config=""):
self.joystickReader.stop_input()
self._active_config = str(config)
self._active_device = str(device)
GuiConfig().set("input_device", self._active_device)
GuiConfig().get("device_config_mapping")[
self._active_device] = self._active_config
self.joystickReader.start_input(self._active_device,
self._active_config)
# update the checked state of the menu items
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._active_config:
c.setChecked(True)
for c in self._menu_devices.actions():
c.setEnabled(True)
if c.text() == self._active_device:
c.setChecked(True)
# update label
if device == "" and config == "":
self._statusbar_label.setText("No input device selected")
elif config == "":
self._statusbar_label.setText("Using [%s] - "
"No input config selected" %
(self._active_device))
else:
self._statusbar_label.setText(
"Using [%s] with config [%s]" %
(self._active_device, self._active_config))
def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=True)
for d in devs:
node = QAction(d["name"], self._menu_devices, checkable=True)
node.toggled.connect(self._inputdevice_selected)
group.addAction(node)
self._menu_devices.addAction(node)
if (d["name"] == GuiConfig().get("input_device")):
self._active_device = d["name"]
if (len(self._active_device) == 0):
self._active_device = self._menu_devices.actions()[0].text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (device_config_mapping):
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[
str(self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
# Now we know what device to use and what mapping, trigger the events
# to change the menus and start the input
for c in self._menu_mappings.actions():
c.setEnabled(True)
if (c.text() == self._current_input_config):
c.setChecked(True)
for c in self._menu_devices.actions():
if (c.text() == self._active_device):
c.setChecked(True)
def _inputdevice_selected(self, checked):
if (not checked):
return
self.joystickReader.stop_input()
sender = self.sender()
self._active_device = sender.text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[str(
self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text(
)
GuiConfig().set("input_device", str(self._active_device))
for c in self._menu_mappings.actions():
if (c.text() == self._current_input_config):
c.setChecked(True)
self.joystickReader.start_input(str(sender.text()),
self._current_input_config)
self._statusbar_label.setText(
"Using [%s] with config [%s]" %
(self._active_device, self._current_input_config))
self.joystickReader.stop_input()
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 setUIState(self, newState, linkURI=""):
self.uiState = newState
if (newState == UIState.DISCONNECTED):
self.setWindowTitle("Not connected")
self.menuItemConnect.setText("Connect to Crazyflie")
self.connectButton.setText("Connect")
self.menuItemQuickConnect.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
if (len(GuiConfig().get("link_uri")) > 0):
self.quickConnectButton.setEnabled(True)
if (newState == UIState.CONNECTED):
s = "Connected on %s" % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.connectButton.setText("Disconnect")
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
if (newState == UIState.CONNECTING):
s = "Connecting to %s ..." % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.connectButton.setText("Cancel")
self.quickConnectButton.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.menuItemQuickConnect.setEnabled(False)
def setUIState(self, newState, linkURI=""):
self.uiState = newState
if (newState == UIState.DISCONNECTED):
self.setWindowTitle("Not connected")
self.menuItemConnect.setText("Connect to Crazyflie")
self.connectButton.setText("Connect")
self.menuItemQuickConnect.setEnabled(True)
self.batteryBar.setValue(3000)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
if (len(GuiConfig().get("link_uri")) > 0):
self.quickConnectButton.setEnabled(True)
if (newState == UIState.CONNECTED):
s = "Connected on %s" % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.connectButton.setText("Disconnect")
self.logConfigAction.setEnabled(True)
if (newState == UIState.CONNECTING):
s = "Connecting to %s ..." % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.connectButton.setText("Cancel")
self.quickConnectButton.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.menuItemQuickConnect.setEnabled(False)
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 readConfigAction(self):
self.statusChanged.emit("Reading config block...", 0)
data = self.loader.read_flash(self.loader.start_page +
self._config_page)
if (data is not None):
self.statusChanged.emit("Reading config block...done!", 100)
if data[0:4] == "0xBC":
# Skip 0xBC and version at the beginning
[channel, speed, pitchTrim,
rollTrim] = struct.unpack("<BBff", data[5:15])
else:
channel = GuiConfig().get("default_cf_channel")
speed = GuiConfig().get("default_cf_speed")
pitchTrim = GuiConfig().get("default_cf_trim")
rollTrim = GuiConfig().get("default_cf_trim")
self.updateConfigSignal.emit(channel, speed, rollTrim, pitchTrim)
else:
self.statusChanged.emit("Reading config block failed!", 0)
def _inputdevice_selected(self, checked):
if not checked:
return
self.joystickReader.stop_input()
sender = self.sender()
self._active_device = sender.text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if self._active_device in device_config_mapping.keys():
self._current_input_config = device_config_mapping[str(self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
GuiConfig().set("input_device", str(self._active_device))
for c in self._menu_mappings.actions():
if c.text() == self._current_input_config:
c.setChecked(True)
self.joystickReader.start_input(str(sender.text()), self._current_input_config)
self._statusbar_label.setText("Using [%s] with config [%s]" % (self._active_device, self._current_input_config))
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
self.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup loggingblock!")
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.addVariable(LogVariable("pm.vbat", "float"))
self.log = self.cf.log.create_log_packet(lg)
if (self.log != None):
self.log.dataReceived.add_callback(self.batteryUpdatedSignal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
logger.warning("Could not setup loggingblock!")
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
GuiConfig().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: %s", checked)
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 __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 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)
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)
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 _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 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 __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 closeEvent(self, event):
self.hide()
self.cf.close_link()
GuiConfig().save_file()
def quickConnect(self):
try:
self.cf.open_link(GuiConfig().get("link_uri"))
except KeyError:
self.cf.open_link("")
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 __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)
self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._emergency_stop_updated_signal.emit)
self._imu_data_signal.connect(self._imu_data_received)
self._motor_data_signal.connect(self._motor_data_received)
# 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(
"flightctrl.xmode", str(enabled)))
self.helper.cf.param.add_update_callback(
"flightctrl.xmode",
lambda name, checked: self.crazyflieXModeCheckbox.setChecked(
eval(checked)))
self.ratePidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightctrl.ratepid", str(enabled)))
self.angularPidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightctrl.ratepid", str(not enabled)))
self.helper.cf.param.add_update_callback(
"flightctrl.ratepid",
lambda name, checked: self.ratePidRadioButton.setChecked(
eval(checked)))
self.ai = AttitudeIndicator()
self.gridLayout.addWidget(self.ai, 0, 1)
self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(
enable_debug_driver=GuiConfig().get("enable_debug_driver"))
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader(cf=self.cf)
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
self._update_input
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self.connectionFailed)
self._input_device_error_signal.connect(self.inputDeviceError)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Connect UI signals
self.menuItemConnect.triggered.connect(self.connectButtonClicked)
self.logConfigAction.triggered.connect(self.doLogConfigDialogue)
self.connectButton.clicked.connect(self.connectButtonClicked)
self.quickConnectButton.clicked.connect(self.quickConnect)
self.menuItemQuickConnect.triggered.connect(self.quickConnect)
self.menuItemConfInputDevice.triggered.connect(self.configInputDevice)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self.updateBatteryVoltage)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(
GuiConfig().get("auto_reconnect"))
# Do not queue data from the controller output to the Crazyflie wrapper
# to avoid latency
#self.joystickReader.sendControlSetpointSignal.connect(
# self.cf.commander.send_setpoint,
# Qt.DirectConnection)
self.joystickReader.input_updated.add_callback(
self.cf.commander.send_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self.connectionLost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI))
self._log_error_signal.connect(self._logging_error)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self.setUIState(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [
dockToolbox,
]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in GuiConfig().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [%s]", e)
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())
