Add Crazyflie 1.0 support

src/cfclient/__init__.py

@@ -25,9 +25,11 @@
 #  Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 
 import json
+import logging
 import os
 from appdirs import AppDirs
 import sys
+from threading import Timer
 
 # Path used all over the application
 if not hasattr(sys, 'frozen'):
@@ -54,3 +56,53 @@
         log_param_doc = json.load(f)
 except (IOError, OSError, json.JSONDecodeError):
     log_param_doc = None
+
+logger = logging.getLogger(__name__)
+
+
+def _enable_legacy_platform_probe_fallback():
+    try:
+        from cflib.crazyflie.platformservice import PlatformService
+    except ImportError:
+        return
+
+    if getattr(PlatformService, "_cfclient_legacy_probe_patch", False):
+        return
+
+    original_fetch_platform_informations = PlatformService.fetch_platform_informations
+
+    def fetch_platform_informations(self, callback):
+        self._cfclient_platform_info_complete = False
+
+        timer = getattr(self, "_cfclient_platform_info_timer", None)
+        if timer:
+            timer.cancel()
+
+        def complete():
+            if self._cfclient_platform_info_complete:
+                return
+
+            self._cfclient_platform_info_complete = True
+            timer = getattr(self, "_cfclient_platform_info_timer", None)
+            if timer:
+                timer.cancel()
+                self._cfclient_platform_info_timer = None
+
+            callback()
+
+        def on_timeout():
+            logger.info("Platform info fetch timed out, assuming legacy firmware")
+            self._protocolVersion = -1
+            complete()
+
+        self._cfclient_platform_info_timer = Timer(1.0, on_timeout)
+        self._cfclient_platform_info_timer.daemon = True
+        self._cfclient_platform_info_timer.start()
+
+        original_fetch_platform_informations(self, complete)
+
+    PlatformService.fetch_platform_informations = fetch_platform_informations
+    PlatformService._cfclient_legacy_probe_patch = True
+
+
+_enable_legacy_platform_probe_fallback()

src/cfclient/ui/main.py

@@ -570,12 +570,13 @@ def _update_battery(self, timestamp, data, logconf):
         self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
 
         color = UiUtils.COLOR_BLUE
-        # TODO firmware reports fully-charged state as 'Battery',
-        # rather than 'Charged'
-        if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
-            color = UiUtils.COLOR_GREEN
-        elif data["pm.state"] == BatteryStates.LOW_POWER:
-            color = UiUtils.COLOR_RED
+        if "pm.state" in data:
+            # TODO firmware reports fully-charged state as 'Battery',
+            # rather than 'Charged'
+            if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
+                color = UiUtils.COLOR_GREEN
+            elif data["pm.state"] == BatteryStates.LOW_POWER:
+                color = UiUtils.COLOR_RED
 
         self.batteryBar.setStyleSheet(UiUtils.progressbar_stylesheet(color))
         self._aff_volts.setText(("%.3f" % data["pm.vbat"]))
@@ -588,7 +589,8 @@ def _connected(self):
 
         lg = LogConfig("Battery", 1000)
         lg.add_variable("pm.vbat", "float")
-        lg.add_variable("pm.state", "int8_t")
+        if self.cf.log.toc.get_element_by_complete_name("pm.state"):
+            lg.add_variable("pm.state", "int8_t")
         try:
             self.cf.log.add_config(lg)
             lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)

src/cfclient/ui/pose_logger.py

@@ -48,6 +48,10 @@ class PoseLogger:
     LOG_NAME_ESTIMATE_ROLL = 'stateEstimate.roll'
     LOG_NAME_ESTIMATE_PITCH = 'stateEstimate.pitch'
     LOG_NAME_ESTIMATE_YAW = 'stateEstimate.yaw'
+    LOG_NAME_STABILIZER_ROLL = 'stabilizer.roll'
+    LOG_NAME_STABILIZER_PITCH = 'stabilizer.pitch'
+    LOG_NAME_STABILIZER_YAW = 'stabilizer.yaw'
+    LOG_NAME_BARO_ASL_LONG = 'baro.aslLong'
     NO_POSE = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
 
     def __init__(self, cf: Crazyflie) -> None:
@@ -62,6 +66,8 @@ def __init__(self, cf: Crazyflie) -> None:
         # X, Y, Z, roll, pitch, yaw
         # roll, pitch and yaw in degrees
         self.pose = self.NO_POSE
+        self._use_legacy_pose = False
+        self._has_legacy_baro = False
 
     @property
     def position(self):
@@ -80,12 +86,35 @@ def rpy_rad(self):
 
     def _connected(self, link_uri) -> None:
         logConf = LogConfig("Pose", 40)
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_X, "float")
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_Y, "float")
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_Z, "float")
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_ROLL, "float")
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_PITCH, "float")
-        logConf.add_variable(self.LOG_NAME_ESTIMATE_YAW, "float")
+
+        has_full_pose = all([
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_X),
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_Y),
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_Z),
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_ROLL),
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_PITCH),
+            self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_ESTIMATE_YAW),
+        ])
+
+        self._use_legacy_pose = not has_full_pose
+        self._has_legacy_baro = False
+
+        if has_full_pose:
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_X, "float")
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_Y, "float")
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_Z, "float")
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_ROLL, "float")
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_PITCH, "float")
+            logConf.add_variable(self.LOG_NAME_ESTIMATE_YAW, "float")
+        else:
+            logConf.add_variable(self.LOG_NAME_STABILIZER_ROLL, "float")
+            logConf.add_variable(self.LOG_NAME_STABILIZER_PITCH, "float")
+            logConf.add_variable(self.LOG_NAME_STABILIZER_YAW, "float")
+
+            self._has_legacy_baro = bool(
+                self._cf.log.toc.get_element_by_complete_name(self.LOG_NAME_BARO_ASL_LONG))
+            if self._has_legacy_baro:
+                logConf.add_variable(self.LOG_NAME_BARO_ASL_LONG, "float")
 
         try:
             self._cf.log.add_config(logConf)
@@ -101,14 +130,28 @@ def _disconnected(self, link_uri) -> None:
         self.pose = self.NO_POSE
 
     def _data_received(self, timestamp, data, logconf) -> None:
-        self.pose = (
-            data[self.LOG_NAME_ESTIMATE_X],
-            data[self.LOG_NAME_ESTIMATE_Y],
-            data[self.LOG_NAME_ESTIMATE_Z],
-            data[self.LOG_NAME_ESTIMATE_ROLL],
-            data[self.LOG_NAME_ESTIMATE_PITCH],
-            data[self.LOG_NAME_ESTIMATE_YAW],
-        )
+        if self._use_legacy_pose:
+            z_estimate = 0.0
+            if self._has_legacy_baro:
+                z_estimate = data[self.LOG_NAME_BARO_ASL_LONG]
+
+            self.pose = (
+                0.0,
+                0.0,
+                z_estimate,
+                data[self.LOG_NAME_STABILIZER_ROLL],
+                data[self.LOG_NAME_STABILIZER_PITCH],
+                data[self.LOG_NAME_STABILIZER_YAW],
+            )
+        else:
+            self.pose = (
+                data[self.LOG_NAME_ESTIMATE_X],
+                data[self.LOG_NAME_ESTIMATE_Y],
+                data[self.LOG_NAME_ESTIMATE_Z],
+                data[self.LOG_NAME_ESTIMATE_ROLL],
+                data[self.LOG_NAME_ESTIMATE_PITCH],
+                data[self.LOG_NAME_ESTIMATE_YAW],
+            )
 
         self.data_received_cb.call(self, self.pose)
 

src/cfclient/ui/tabs/FlightTab.py

@@ -421,7 +421,7 @@ def _update_flight_commander(self, connected):
         #                  flowV1    flowV2     LightHouse       LPS
         position_decks = ['bcFlow', 'bcFlow2', 'bcLighthouse4', 'bcLoco', 'bcDWM1000']
         for deck in position_decks:
-            if int(self._helper.cf.param.values['deck'][deck]) == 1:
+            if self._deck_param_enabled(deck):
                 self.commanderBox.setEnabled(True)
                 break
         else:
@@ -569,6 +569,15 @@ def maxAngleChanged(self):
         if (self.isInCrazyFlightmode is True):
             Config().set("max_rp", self.maxAngle.value())
 
+    def _deck_param_enabled(self, name):
+        try:
+            return int(self._helper.cf.param.values.get("deck", {}).get(name, 0)) == 1
+        except (TypeError, ValueError):
+            return False
+
+    def _int_config(self, key):
+        return int(Config().get(key))
+
     def _trim_pitch_changed(self, value):
         logger.debug("Pitch trim updated to [%f]" % value)
         self._helper.inputDeviceReader.trim_pitch = value
@@ -631,21 +640,21 @@ def flightmodeChange(self, item):
                      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.maxAngle.setValue(self._int_config("normal_max_rp"))
+            self.maxThrust.setValue(self._int_config("normal_max_thrust"))
+            self.minThrust.setValue(self._int_config("normal_min_thrust"))
+            self.slewEnableLimit.setValue(self._int_config("normal_slew_limit"))
             self.thrustLoweringSlewRateLimit.setValue(
-                Config().get("normal_slew_rate"))
-            self.maxYawRate.setValue(Config().get("normal_max_yaw"))
+                self._int_config("normal_slew_rate"))
+            self.maxYawRate.setValue(self._int_config("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.maxAngle.setValue(self._int_config("max_rp"))
+            self.maxThrust.setValue(self._int_config("max_thrust"))
+            self.minThrust.setValue(self._int_config("min_thrust"))
+            self.slewEnableLimit.setValue(self._int_config("slew_limit"))
             self.thrustLoweringSlewRateLimit.setValue(
-                Config().get("slew_rate"))
-            self.maxYawRate.setValue(Config().get("max_yaw"))
+                self._int_config("slew_rate"))
+            self.maxYawRate.setValue(self._int_config("max_yaw"))
             self.isInCrazyFlightmode = True
 
         if (item == 0):
@@ -710,20 +719,20 @@ def _populate_assisted_mode_dropdown(self):
         heightHoldPossible = False
         hoverPossible = False
 
-        if int(self._helper.cf.param.values["deck"]["bcZRanger"]) == 1:
+        if self._deck_param_enabled("bcZRanger"):
             heightHoldPossible = True
             self._helper.inputDeviceReader.set_hover_max_height(1.0)
 
-        if int(self._helper.cf.param.values["deck"]["bcZRanger2"]) == 1:
+        if self._deck_param_enabled("bcZRanger2"):
             heightHoldPossible = True
             self._helper.inputDeviceReader.set_hover_max_height(2.0)
 
-        if int(self._helper.cf.param.values["deck"]["bcFlow"]) == 1:
+        if self._deck_param_enabled("bcFlow"):
             heightHoldPossible = True
             hoverPossible = True
             self._helper.inputDeviceReader.set_hover_max_height(1.0)
 
-        if int(self._helper.cf.param.values["deck"]["bcFlow2"]) == 1:
+        if self._deck_param_enabled("bcFlow2"):
             heightHoldPossible = True
             hoverPossible = True
             self._helper.inputDeviceReader.set_hover_max_height(2.0)

src/cfclient/ui/tabs/LEDRingTab.py

@@ -55,6 +55,12 @@ class LEDRingTab(TabToolbox, led_ring_tab_class):
     _connected_signal = pyqtSignal(str)
     _disconnected_signal = pyqtSignal(str)
 
+    def _deck_param_enabled(self, name):
+        try:
+            return int(self._helper.cf.param.values.get("deck", {}).get(name, 0)) == 1
+        except (TypeError, ValueError):
+            return False
+
     def __init__(self, helper):
         super(LEDRingTab, self).__init__(helper, 'LED Ring')
         self.setupUi(self)
@@ -239,8 +245,9 @@ def _ring_populate_dropdown(self):
 
         self._led_ring_effect.currentIndexChanged.connect(self._ring_effect_changed)
         self._led_ring_effect.setCurrentIndex(current)
-        self._led_ring_effect.setEnabled(int(self._helper.cf.param.values["deck"]["bcLedRing"]) == 1)
-        self._led_ring_headlight.setEnabled(int(self._helper.cf.param.values["deck"]["bcLedRing"]) == 1)
+        led_ring_enabled = self._deck_param_enabled("bcLedRing")
+        self._led_ring_effect.setEnabled(led_ring_enabled)
+        self._led_ring_headlight.setEnabled(led_ring_enabled)
 
         try:
             self._helper.cf.param.set_value("ring.effect", "13")

src/cfclient/ui/tabs/lighthouse_tab.py

@@ -391,7 +391,13 @@ def _connected(self, link_uri):
         self._basestation_geometry_dialog.reset()
         self._is_connected = True
 
-        if self._helper.cf.param.get_value('deck.bcLighthouse4') == '1':
+        try:
+            lighthouse_deck_enabled = self._helper.cf.param.get_value(
+                'deck.bcLighthouse4') == '1'
+        except KeyError:
+            lighthouse_deck_enabled = False
+
+        if lighthouse_deck_enabled:
             self._lighthouse_deck_detected()
 
         self._update_ui()