Merge pull request #682 from bitcraze/krichardsson/issue-668

Show supervisor info
bitcraze · Oct 17, 2023 · 4edd71a · 4edd71a
2 parents eee4ae1 + b09dce8
commit 4edd71a
Show file tree

Hide file tree

Showing 2 changed files with 340 additions and 224 deletions.
diff --git a/src/cfclient/ui/tabs/FlightTab.py b/src/cfclient/ui/tabs/FlightTab.py
@@ -39,9 +39,9 @@
 import cfclient
 from cfclient.ui.widgets.ai import AttitudeIndicator
 
-from cfclient.utils.config import Config
 from cflib.crazyflie.log import LogConfig
 
+from cfclient.utils.config import Config
 from cfclient.utils.input import JoystickReader
 
 from cfclient.ui.tab_toolbox import TabToolbox
@@ -191,7 +191,7 @@ def __init__(self, helper):
         # Supervisor
         self._supervisor_info_bitfield = 0
         self.armButton.clicked.connect(self.updateArm)
-        self._update_arm_button(False)
+        self._update_supervisor_and_arming(False)
 
         self.uiSetupReady()
 
@@ -301,7 +301,7 @@ def _log_data_received(self, timestamp, data, logconf):
             if self.LOG_NAME_SUPERVISOR_INFO in data:
                 self._supervisor_info_bitfield = data[self.LOG_NAME_SUPERVISOR_INFO]
 
-            self._update_arm_button(True)
+            self._update_supervisor_and_arming(True)
 
     def _pose_data_received(self, pose_logger, pose):
         if self.isVisible():
@@ -355,17 +355,30 @@ def _change_input_labels(self, using_hover_assist):
         self.inputRollLabel.setText(roll)
         self.inputYawLabel.setText(yaw)
 
-    def _update_arm_button(self, connected):
+    def _update_supervisor_and_arming(self, connected):
         if not connected:
             self.armButton.setStyleSheet("")
             self.armButton.setText("Arm")
             self.armButton.setEnabled(False)
+            self._supervisor_state.setText("")
+            self._supervisor_state.setStyleSheet("")
             return
 
+        self._supervisor_state.setText("")
+        if self._is_tumbled():
+            self._supervisor_state.setText("Tumbled")
+
+        if self._is_locked():
+            self._supervisor_state.setText("Locked-please reboot")
+            self._supervisor_state.setStyleSheet("background-color: red")
+        else:
+            self._supervisor_state.setStyleSheet("")
+
         if self._is_flying():
             self.armButton.setEnabled(True)
             self.armButton.setText("Emergency stop")
             self.armButton.setStyleSheet("background-color: red")
+            self._supervisor_state.setText("Flying")
             return
 
         if self._is_armed():
@@ -434,6 +447,11 @@ def connected(self, linkURI):
         # Add supervisor info if it exists to keep backwards compatibility
         if self._helper.cf.log.toc.get_element_by_complete_name(self.LOG_NAME_SUPERVISOR_INFO):
             lg.add_variable(self.LOG_NAME_SUPERVISOR_INFO)
+        # Full supervisor info available after V7, hide supervisor info for earlier versions
+        update_supervisor_info = self._helper.cf.platform.get_protocol_version() >= 7
+        self._supervisor_state.setVisible(update_supervisor_info)
+        self._supervisor_label1.setVisible(update_supervisor_info)
+        self._supervisor_label2.setVisible(update_supervisor_info)
 
         try:
             self._helper.cf.log.add_config(lg)
@@ -503,7 +521,7 @@ def disconnected(self, linkURI):
         self._update_flight_commander(False)
 
         self._supervisor_info_bitfield = 0
-        self._update_arm_button(False)
+        self._update_supervisor_and_arming(False)
 
     def _can_arm(self):
         return bool(self._supervisor_info_bitfield & 0x0001)
@@ -523,6 +541,9 @@ def _is_flying(self):
     def _is_tumbled(self):
         return bool(self._supervisor_info_bitfield & 0x0020)
 
+    def _is_locked(self):
+        return bool(self._supervisor_info_bitfield & 0x0040)
+
     def minMaxThrustChanged(self):
         self._helper.inputDeviceReader.min_thrust = self.minThrust.value()
         self._helper.inputDeviceReader.max_thrust = self.maxThrust.value()
@@ -685,7 +706,7 @@ def _all_params_updated(self):
         self._ring_populate_dropdown()
         self._populate_assisted_mode_dropdown()
         self._update_flight_commander(True)
-        self._update_arm_button(True)
+        self._update_supervisor_and_arming(True)
 
     def _ring_populate_dropdown(self):
         try: