class PlotTab(Tab, plot_tab_class):
"""Tab for plotting logging data"""
_log_data_signal = pyqtSignal(int, object, object)
_log_error_signal = pyqtSignal(object, str)
_disconnected_signal = pyqtSignal(str)
colors = ['g', 'b', 'm', 'r', 'y', 'c']
def __init__(self, tabWidget, helper, *args):
super(PlotTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Plotter"
self.menuName = "Plotter"
self._log_error_signal.connect(self._logging_error)
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._log_data_signal.connect(self._log_data_received)
self.tabWidget = tabWidget
self.helper = helper
self.plotLayout.addWidget(self._plot)
# Connect external signals if we can use the tab
if self.enabled:
self._disconnected_signal.connect(self._disconnected)
self.helper.cf.disconnected.add_callback(
self._disconnected_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
def _disconnected(self, link_uri):
"""Callback for when the Crazyflie has been disconnected"""
self._model.reset()
self._plot.removeAllDatasets()
self._plot.set_title("")
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._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 _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 PlotTab(Tab, plot_tab_class):
"""Tab for plotting logging data"""
_log_data_signal = pyqtSignal(int, object, object)
_log_error_signal = pyqtSignal(object, str)
_disconnected_signal = pyqtSignal(str)
_connected_signal = pyqtSignal(str)
colors = ['g', 'b', 'm', 'r', 'y', 'c']
def __init__(self, tabWidget, helper, *args):
super(PlotTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Plotter"
self.menuName = "Plotter"
self._log_error_signal.connect(self._logging_error)
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._log_data_signal.connect(self._log_data_received)
self.tabWidget = tabWidget
self.helper = helper
self.plotLayout.addWidget(self._plot)
# Connect external signals if we can use the tab
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
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.reset()
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._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 _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 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 PlotTab(Tab, plot_tab_class):
"""Tab for plotting logging data"""
logDataSignal = pyqtSignal(object, int)
_log_error_signal = pyqtSignal(object, str)
colors = ['g', 'b', 'm', 'r', 'y', 'c']
dsList = []
connectedSignal = pyqtSignal(str)
logConfigReadSignal = pyqtSignal()
def __init__(self, tabWidget, helper, *args):
super(PlotTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Plotter"
self.menuName = "Plotter"
self.previousLog = None
self._log_error_signal.connect(self._logging_error)
self.dsList = helper.logConfigReader.getLogConfigs()
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.dataSelector.currentIndexChanged.connect(self.newLogSetupSelected)
self.logDataSignal.connect(self.logDataReceived)
self.tabWidget = tabWidget
self.helper = helper
# self.layout().addWidget(self.dataSelector)
self.plotLayout.addWidget(self.plot)
# Connect external signals if we can use the tab
if self.enabled:
self.helper.cf.connectSetupFinished.add_callback(
self.connectedSignal.emit)
self.connectedSignal.connect(self.connected)
self.helper.cf.logConfigRead.add_callback(self.logConfigReadSignal.emit)
self.logConfigReadSignal.connect(self.logConfigChanged)
self.datasets = []
self.logEntrys = []
def newLogSetupSelected(self, item):
if (len(self.logEntrys) > 0):
log = self.logEntrys[item]
if (self.previousLog != None):
self.previousLog.stop()
log.start()
self.previousLog = log
# Setup the plot
self.plot.removeAllDatasets()
self.datasets = []
colorSelector = 0
info = self.dsList[item]
self.plot.setTitle(info.getName())
for d in info.getVariables():
self.plot.add_curve(d.getName(), self.colors[colorSelector % len(self.colors)])
colorSelector += 1
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Plot error", "Error when starting log config"
" [%s]: %s" % (log_conf.getName(), msg))
def connected(self, link):
self.populateLogConfigs()
def populateLogConfigs(self):
prevSelected = self.dataSelector.currentText()
self.dataSelector.blockSignals(True)
self.dataSelector.clear()
self.dataSelector.blockSignals(False)
for d in self.dsList:
logEntry = self.helper.cf.log.create_log_packet(d)
if (logEntry != None):
self.dataSelector.blockSignals(True)
self.dataSelector.addItem(d.getName())
self.dataSelector.blockSignals(False)
self.logEntrys.append(logEntry)
logEntry.data_received.add_callback(self.logDataSignal.emit)
logEntry.error.add_callback(self._log_error_signal.emit)
else:
logger.warning("Could not setup log configuration!")
if (len(self.logEntrys) > 0):
prevIndex = self.dataSelector.findText(prevSelected)
if prevIndex >= 0:
self.dataSelector.setCurrentIndex(prevIndex)
else:
self.dataSelector.currentIndexChanged.emit(0)
def logDataReceived(self, data, timestamp):
try:
dataIndex = 0
self.plot.add_data(data, timestamp)
except Exception as e:
# When switching what to log we might still get logging packets...
# and that will not be pretty so let's just ignore the problem ;-)
logger.warning("Exception for plot data: %s", e)
import traceback
logger.info(traceback.format_exc())
def logConfigChanged(self):
self.dsList = self.helper.logConfigReader.getLogConfigs()
self.populateLogConfigs()