Merge pull request #581 from bitcraze/Aris/crtp_supervisor_lib

Supervisor subsystem

Author: gemenerik

cflib/crazyflie/__init__.py

@@ -51,6 +51,7 @@
 from .mem import Memory
 from .param import Param
 from .platformservice import PlatformService
+from .supervisor import Supervisor
 from .toccache import TocCache
 from cflib.crazyflie.high_level_commander import HighLevelCommander
 from cflib.utils.callbacks import Caller
@@ -124,6 +125,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None):
         self.platform = PlatformService(self)
         self.appchannel = Appchannel(self)
         self.link_statistics = LinkStatistics(self)
+        self.supervisor = Supervisor(self)
 
         self.link_uri = ''
 

cflib/crazyflie/localization.py

@@ -28,6 +28,7 @@
 import collections
 import logging
 import struct
+import warnings
 
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
@@ -58,8 +59,8 @@ class Localization():
     RANGE_STREAM_REPORT = 0
     RANGE_STREAM_REPORT_FP16 = 1
     LPS_SHORT_LPP_PACKET = 2
-    EMERGENCY_STOP = 3
-    EMERGENCY_STOP_WATCHDOG = 4
+    EMERGENCY_STOP = 3          # Deprecated: use supervisor.send_emergency_stop()
+    EMERGENCY_STOP_WATCHDOG = 4  # Deprecated: use supervisor.send_emergency_stop_watchdog()
     COMM_GNSS_NMEA = 6
     COMM_GNSS_PROPRIETARY = 7
     EXT_POSE = 8
@@ -169,25 +170,53 @@ def send_short_lpp_packet(self, dest_id, data):
 
     def send_emergency_stop(self):
         """
-        Send emergency stop
-        """
-
-        pk = CRTPPacket()
-        pk.port = CRTPPort.LOCALIZATION
-        pk.channel = self.GENERIC_CH
-        pk.data = struct.pack('<B', self.EMERGENCY_STOP)
-        self._cf.send_packet(pk)
+        Send emergency stop (deprecated).
+
+        Deprecated:
+            Use ``cf.supervisor.send_emergency_stop()`` instead.
+            If the connected Crazyflie does not support CRTP protocol version 12
+            or later, the legacy localization channel is used as a fallback.
+        """
+        warnings.warn(
+            'localization.send_emergency_stop() is deprecated. '
+            'Use cf.supervisor.send_emergency_stop() instead. '
+            'Update your Crazyflie firmware to use CRTP protocol version 12 or later.',
+            DeprecationWarning,
+            stacklevel=2,
+        )
+        if self._cf.platform.get_protocol_version() >= 12:
+            self._cf.supervisor.send_emergency_stop()
+        else:
+            pk = CRTPPacket()
+            pk.port = CRTPPort.LOCALIZATION
+            pk.channel = self.GENERIC_CH
+            pk.data = struct.pack('<B', self.EMERGENCY_STOP)
+            self._cf.send_packet(pk)
 
     def send_emergency_stop_watchdog(self):
         """
-        Send emergency stop watchdog
-        """
-
-        pk = CRTPPacket()
-        pk.port = CRTPPort.LOCALIZATION
-        pk.channel = self.GENERIC_CH
-        pk.data = struct.pack('<B', self.EMERGENCY_STOP_WATCHDOG)
-        self._cf.send_packet(pk)
+        Send emergency stop watchdog (deprecated).
+
+        Deprecated:
+            Use ``cf.supervisor.send_emergency_stop_watchdog()`` instead.
+            If the connected Crazyflie does not support CRTP protocol version 12
+            or later, the legacy localization channel is used as a fallback.
+        """
+        warnings.warn(
+            'localization.send_emergency_stop_watchdog() is deprecated. '
+            'Use cf.supervisor.send_emergency_stop_watchdog() instead. '
+            'Update your Crazyflie firmware to use CRTP protocol version 12 or later.',
+            DeprecationWarning,
+            stacklevel=2,
+        )
+        if self._cf.platform.get_protocol_version() >= 12:
+            self._cf.supervisor.send_emergency_stop_watchdog()
+        else:
+            pk = CRTPPacket()
+            pk.port = CRTPPort.LOCALIZATION
+            pk.channel = self.GENERIC_CH
+            pk.data = struct.pack('<B', self.EMERGENCY_STOP_WATCHDOG)
+            self._cf.send_packet(pk)
 
     def send_lh_persist_data_packet(self, geo_list, calib_list):
         """

cflib/crazyflie/platformservice.py

@@ -26,6 +26,7 @@
 Used for sending control setpoints to the Crazyflie
 """
 import logging
+import warnings
 
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
@@ -40,8 +41,6 @@
 APP_CHANNEL = 2
 
 PLATFORM_SET_CONT_WAVE = 0
-PLATFORM_REQUEST_ARMING = 1
-PLATFORM_REQUEST_CRASH_RECOVERY = 2
 
 VERSION_GET_PROTOCOL = 0
 VERSION_GET_FIRMWARE = 1
@@ -90,25 +89,38 @@ def set_continous_wave(self, enabled):
 
     def send_arming_request(self, do_arm: bool):
         """
-        Send system arm/disarm request
+        Send system arm/disarm request (deprecated).
 
         Args:
             do_arm (bool): True = arm the system, False = disarm the system
+
+        Deprecated:
+            Use `supervisor.send_arming_request(do_arm)` instead.
         """
-        pk = CRTPPacket()
-        pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
-        pk.data = (PLATFORM_REQUEST_ARMING, do_arm)
-        self._cf.send_packet(pk)
+        warnings.warn(
+            'platform.send_arming_request is deprecated. '
+            'Use supervisor.send_arming_request(do_arm) instead.',
+            category=DeprecationWarning,
+            stacklevel=2
+        )
+
+        self._cf.supervisor.send_arming_request(do_arm)
 
     def send_crash_recovery_request(self):
         """
-        Send crash recovery request
+        Send crash recovery request (deprecated).
 
+        Deprecated:
+            Use `supervisor.send_crash_recovery_request()` instead.
         """
-        pk = CRTPPacket()
-        pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
-        pk.data = (PLATFORM_REQUEST_CRASH_RECOVERY, )
-        self._cf.send_packet(pk)
+        warnings.warn(
+            'platform.send_crash_recovery_request is deprecated. '
+            'Use supervisor.send_crash_recovery_request() instead.',
+            category=DeprecationWarning,
+            stacklevel=2
+        )
+
+        self._cf.supervisor.send_crash_recovery_request()
 
     def get_protocol_version(self):
         """