diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 5e09b906d..89bfedf1c 100644 --- a/cflib/crazyflie/__init__.py +++ b/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 = '' diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 1d50d81d9..f032f1144 100644 --- a/cflib/crazyflie/localization.py +++ b/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('= 12: + self._cf.supervisor.send_emergency_stop() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack('= 12: + self._cf.supervisor.send_emergency_stop_watchdog() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack('. +""" +Provides access to the supervisor module of the Crazyflie platform. +""" +import logging +import time +import warnings + +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + +__author__ = 'Bitcraze AB' +__all__ = ['Supervisor'] + +logger = logging.getLogger(__name__) + +SUPERVISOR_CH_INFO = 0 +SUPERVISOR_CH_COMMAND = 1 + +# Bit positions +BIT_CAN_BE_ARMED = 0 +BIT_IS_ARMED = 1 +BIT_IS_AUTO_ARMED = 2 +BIT_CAN_FLY = 3 +BIT_IS_FLYING = 4 +BIT_IS_TUMBLED = 5 +BIT_IS_LOCKED = 6 +BIT_IS_CRASHED = 7 +BIT_HL_CONTROL_ACTIVE = 8 +BIT_HL_TRAJ_FINISHED = 9 +BIT_HL_CONTROL_DISABLED = 10 + +CMD_GET_STATE_BITFIELD = 0x0C + +CMD_ARM_SYSTEM = 0x01 +CMD_RECOVER_SYSTEM = 0x02 +CMD_EMERGENCY_STOP = 0x03 +CMD_EMERGENCY_STOP_WATCHDOG = 0x04 + + +class Supervisor: + """ + This class provides two main functionalities: + + 1. **Reading the system state** + Accesses the Crazyflie's supervisor bitfield to determine the current state, + such as whether it can be armed, is flying or crashed. + + 2. **Sending system commands** + Sends arming/disarming requests and crash recovery commands to the Crazyflie. + """ + STATES = [ + 'Can be armed', + 'Is armed', + 'Is auto armed', + 'Can fly', + 'Is flying', + 'Is tumbled', + 'Is locked', + 'Is crashed', + 'HL control is active', + 'Finished HL trajectory', + 'HL control is disabled' + ] + + def __init__(self, crazyflie): + self._cf = crazyflie + + self._cache_timeout = 0.1 # seconds + self._last_fetch_time = 0 + self._bitfield = None + self._cf.add_port_callback(CRTPPort.SUPERVISOR, self._supervisor_callback) + self._bitfield_response_received = False + + def _check_protocol_version(self): + """Returns True if the protocol version is supported, False otherwise.""" + version = self._cf.platform.get_protocol_version() + if version < 12: + warnings.warn( + 'The supervisor subsystem requires CRTP protocol version 12 or later. ' + f'Connected Crazyflie reports version {version}. ' + 'Update your Crazyflie firmware.', + stacklevel=3, + ) + return False + return True + + def _supervisor_callback(self, pk: CRTPPacket): + """ + Called when a packet is received. + """ + if len(pk.data) < 1: + return + + cmd = pk.data[0] + if cmd & 0x80: # high bit = response + orig_cmd = cmd & 0x7F + if orig_cmd == CMD_GET_STATE_BITFIELD: + self._bitfield = int.from_bytes(pk.data[1:], byteorder='little') + self._bitfield_response_received = True + logger.info(f'Supervisor bitfield received: 0x{self._bitfield:04X}') + + elif orig_cmd == CMD_ARM_SYSTEM: + success = bool(pk.data[1]) + is_armed = bool(pk.data[2]) + logger.info(f'Arm response: success={success}, is_armed={is_armed}') + + elif orig_cmd == CMD_RECOVER_SYSTEM: + success = bool(pk.data[1]) + recovered = bool(pk.data[2]) + logger.info(f'Recovery response: success={success}, recovered={recovered}') + + def _fetch_bitfield(self, timeout=0.2): + """ + Request the bitfield and wait for response (blocking). + Uses time-based cache to avoid sending packages too frequently. + """ + if not self._check_protocol_version(): + return 0 + now = time.time() + + # Return cached value if it's recent enough + if self._bitfield is not None and (now - self._last_fetch_time) < self._cache_timeout: + return self._bitfield + + # Send a new request + self._bitfield_response_received = False + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_INFO) + pk.data = [CMD_GET_STATE_BITFIELD] + self._cf.send_packet(pk) + + # Wait for response + start_time = now + while not self._bitfield_response_received: + if time.time() - start_time > timeout: + logger.warning('Timeout waiting for supervisor bitfield response') + return self._bitfield or 0 # still return last known value + time.sleep(0.01) + + # Update timestamp + self._last_fetch_time = time.time() + return self._bitfield or 0 + + def _bit(self, position): + bitfield = self._fetch_bitfield() + return bool((bitfield >> position) & 0x01) + + def read_bitfield(self): + """ + Directly get the full bitfield value. + """ + return self._fetch_bitfield() + + def read_state_list(self): + """ + Reads the bitfield and returns the list of all active states. + """ + bitfield = self.read_bitfield() + list = self.decode_bitfield(bitfield) + return list + + def decode_bitfield(self, value): + """ + Given a bitfield integer `value` and a list of `self.STATES`, + returns the names of all states whose bits are set. + Bit 0 corresponds to states[0], Bit 1 to states[1], etc. + * Bit 0 = Can be armed - the system can be armed and will accept an arming command. + * Bit 1 = Is armed - the system is armed. + * Bit 2 = Is auto armed - the system is configured to automatically arm. + * Bit 3 = Can fly - the Crazyflie is ready to fly. + * Bit 4 = Is flying - the Crazyflie is flying. + * Bit 5 = Is tumbled - the Crazyflie is up side down. + * Bit 6 = Is locked - the Crazyflie is in the locked state and must be restarted. + * Bit 7 = Is crashed - the Crazyflie has crashed. + * Bit 8 = High level control is actively flying the drone. + * Bit 9 = High level trajectory has finished. + * Bit 10 = High level control is disabled and not producing setpoints. + """ + if value < 0: + raise ValueError('value must be >= 0') + + result = [] + for bit_index, name in enumerate(self.STATES): + if value & (1 << bit_index): + result.append(name) + + return result + + @property + def can_be_armed(self): + return self._bit(BIT_CAN_BE_ARMED) + + @property + def is_armed(self): + return self._bit(BIT_IS_ARMED) + + @property + def is_auto_armed(self): + return self._bit(BIT_IS_AUTO_ARMED) + + @property + def can_fly(self): + return self._bit(BIT_CAN_FLY) + + @property + def is_flying(self): + return self._bit(BIT_IS_FLYING) + + @property + def is_tumbled(self): + return self._bit(BIT_IS_TUMBLED) + + @property + def is_locked(self): + return self._bit(BIT_IS_LOCKED) + + @property + def is_crashed(self): + return self._bit(BIT_IS_CRASHED) + + @property + def hl_control_active(self): + return self._bit(BIT_HL_CONTROL_ACTIVE) + + @property + def hl_traj_finished(self): + return self._bit(BIT_HL_TRAJ_FINISHED) + + @property + def hl_control_disabled(self): + return self._bit(BIT_HL_CONTROL_DISABLED) + + def send_arming_request(self, do_arm: bool): + """ + Send system arm/disarm request. + + Args: + do_arm (bool): True = arm the system, False = disarm the system + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_ARM_SYSTEM, do_arm) + self._cf.send_packet(pk) + logger.debug(f'Sent arming request: do_arm={do_arm}') + + def send_crash_recovery_request(self): + """ + Send crash recovery request. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_RECOVER_SYSTEM,) + self._cf.send_packet(pk) + logger.debug('Sent crash recovery request') + + def send_emergency_stop(self): + """ + Send emergency stop. The Crazyflie will immediately stop all motors. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_EMERGENCY_STOP,) + self._cf.send_packet(pk) + logger.debug('Sent emergency stop') + + def send_emergency_stop_watchdog(self): + """ + Send emergency stop watchdog. The Crazyflie will stop all motors + unless this command is repeated at regular intervals. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_EMERGENCY_STOP_WATCHDOG,) + self._cf.send_packet(pk) + logger.debug('Sent emergency stop watchdog') diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index 2cb9118e7..8204764cd 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -47,6 +47,7 @@ class CRTPPort: LOCALIZATION = 0x06 COMMANDER_GENERIC = 0x07 SETPOINT_HL = 0x08 + SUPERVISOR = 0x09 PLATFORM = 0x0D LINKCTRL = 0x0F ALL = 0xFF diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 303f49536..eef11b70c 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -139,7 +139,7 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) take_off_simple(scf) @@ -230,7 +230,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) take_off_simple(scf) @@ -323,7 +323,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) move_linear_simple(scf) @@ -436,7 +436,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() @@ -553,7 +553,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() @@ -684,7 +684,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 908c836ef..17ba26e68 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -185,7 +185,7 @@ def __init__(self, URI): self._imgDownload.start() # Arm the Crazyflie - self.cf.platform.send_arming_request(True) + self.cf.supervisor.send_arming_request(True) time.sleep(1.0) self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} diff --git a/examples/autonomy/autonomous_sequence.py b/examples/autonomy/autonomous_sequence.py index 8c7a1b33c..9ea47b186 100644 --- a/examples/autonomy/autonomous_sequence.py +++ b/examples/autonomy/autonomous_sequence.py @@ -92,7 +92,7 @@ def run_sequence(scf, sequence): cf = scf.cf # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) take_off(cf, sequence[0]) diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index f4f33bd85..45365e0b6 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -92,7 +92,7 @@ def run_sequence(cf, trajectory_id, duration, relative_yaw=False): commander = cf.high_level_commander # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) takeoff_yaw = 3.14 / 2 if relative_yaw else 0.0 diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index 9dff82cf5..065b8da12 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -88,7 +88,7 @@ def run_sequence(cf, trajectory_id, duration): commander = cf.high_level_commander # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) commander.takeoff(1.0, 2.0) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index e1871bca5..f455a3b2b 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -112,7 +112,7 @@ def turn_off_leds(scf): def run_sequence(scf, alpha, r): # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) commander = scf.cf.high_level_commander diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index b5954fc85..8e25ece64 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -84,7 +84,7 @@ def run_sequence(scf): # cf.param.set_value('stabilizer.controller', '2') # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) print('takeoff') diff --git a/examples/autonomy/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py index da86b55b9..abb6194a6 100644 --- a/examples/autonomy/motion_commander_demo.py +++ b/examples/autonomy/motion_commander_demo.py @@ -53,7 +53,7 @@ with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) # We take off when the commander is created diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index 1b0f93796..1fd9c756a 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -44,7 +44,7 @@ def slightly_more_complex_usage(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with PositionHlCommander( @@ -74,7 +74,7 @@ def slightly_more_complex_usage(): def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with PositionHlCommander(scf, @@ -91,7 +91,7 @@ def land_on_elevated_surface(): def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index 632ebc9d7..449b022ce 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -123,7 +123,7 @@ def run_sequence(scf): reset_estimator(scf) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) run_sequence(scf) diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py index 64b5a5812..08b29e06e 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -155,7 +155,7 @@ def run_sequence(scf): # start_position_printing(scf) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) run_sequence(scf) diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index 2c3134ff3..55e5bc626 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -151,8 +151,8 @@ def run_sequence(scf0, scf1): reset_estimator(scf1) # Arm the Crazyflies - scf0.cf.platform.send_arming_request(True) - scf1.cf.platform.send_arming_request(True) + scf0.cf.supervisor.send_arming_request(True) + scf1.cf.supervisor.send_arming_request(True) time.sleep(1.0) run_sequence(scf0, scf1) diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 162a8fd0c..57ddccf3e 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -184,7 +184,7 @@ def run_sequence(cf, trajectory_id, duration): reset_estimator(cf) # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) run_sequence(cf, trajectory_id, duration) diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py index 134586028..706048430 100644 --- a/examples/mocap/mocap_swarm.py +++ b/examples/mocap/mocap_swarm.py @@ -94,7 +94,7 @@ def activate_kalman_estimator(scf: SyncCrazyflie): def arm(scf: SyncCrazyflie): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py index 71ab49778..c480077e3 100644 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ b/examples/mocap/mocap_swarm_multi_commander.py @@ -96,7 +96,7 @@ def activate_kalman_estimator(scf: SyncCrazyflie): def run_sequence(scf: SyncCrazyflie): print('This is: ', scf._link_uri) - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) # .takeoff() is automatic when entering the "with PositionHlCommander" context diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index d109a16ae..62eb0cd88 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -250,7 +250,7 @@ def run_sequence(cf, trajectory_id, duration): reset_estimator(cf) # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) run_sequence(cf, trajectory_id, duration) diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index 8c3e9340f..0ab6eb347 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -60,7 +60,7 @@ def _connected(self, link_uri): has been connected and the TOCs have been downloaded.""" # Arm the Crazyflie - self._cf.platform.send_arming_request(True) + self._cf.supervisor.send_arming_request(True) time.sleep(1.0) # Start a separate thread to do the motor test. diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index 3fe556388..360eeeb6f 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -60,7 +60,7 @@ def _connected(self, link_uri): has been connected and the TOCs have been downloaded.""" # Arm the Crazyflie - self._cf.platform.send_arming_request(True) + self._cf.supervisor.send_arming_request(True) time.sleep(1.0) def _connection_failed(self, link_uri, msg): diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 837c7f570..252717410 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -113,7 +113,7 @@ def __init__(self, URI): self.cf.open_link(URI) # Arm the Crazyflie - self.cf.platform.send_arming_request(True) + self.cf.supervisor.send_arming_request(True) time.sleep(1.0) self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} diff --git a/examples/multiranger/multiranger_push.py b/examples/multiranger/multiranger_push.py index 491f30038..6e2ab8004 100755 --- a/examples/multiranger/multiranger_push.py +++ b/examples/multiranger/multiranger_push.py @@ -75,7 +75,7 @@ def is_close(range): cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with MotionCommander(scf) as motion_commander: diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 7e06a59c3..cc28ed9ce 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -83,7 +83,7 @@ def handle_range_measurement(range): cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with MotionCommander(scf) as motion_commander: diff --git a/examples/positioning/flowsequence_sync.py b/examples/positioning/flowsequence_sync.py index 4f62db71b..8c9e65f2d 100644 --- a/examples/positioning/flowsequence_sync.py +++ b/examples/positioning/flowsequence_sync.py @@ -54,7 +54,7 @@ time.sleep(1) # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) for y in range(10): diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index 79ff83634..2deeddfa1 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -68,7 +68,7 @@ def run_sequence(scf, sequence, base_x, base_y, base_z, yaw): cf = scf.cf # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) for position in sequence: diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index 4de1d7f02..4b2053f55 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -111,7 +111,7 @@ def matrix_print(cf, pc, image_def): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) with PositionHlCommander(scf, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index b557cce2d..58ce5891d 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -125,7 +125,7 @@ def param_deck_flow(_, value_str): sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index b562ac58b..ae098e0f0 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -45,7 +45,7 @@ def light_check(scf: SyncCrazyflie): def arm(scf: SyncCrazyflie): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/supervisor/flying_with_supervisor.py b/examples/supervisor/flying_with_supervisor.py new file mode 100644 index 000000000..d6b9432d8 --- /dev/null +++ b/examples/supervisor/flying_with_supervisor.py @@ -0,0 +1,88 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Simple example showing how to fly the Crazyflie using supervisor state information. +Based on its current state, the Crazyflie will arm (if it can be armed), take +off (if it can fly), and land (if it is flying). Before each action, we call +the supervisor to check if the Crazyflie is crashed, locked or tumbled. +Tested with the Flow deck V2 and the Lighthouse positioning system. +""" +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +def safety_check(sup): + if sup.is_crashed: + raise Exception('Crazyflie crashed!') + if sup.is_locked: + raise Exception('Crazyflie locked!') + if sup.is_tumbled: + raise Exception('Crazyflie tumbled!') + + +def run_sequence(scf: SyncCrazyflie, sup): + commander = scf.cf.high_level_commander + + try: + safety_check(sup) + if sup.can_be_armed: + print('The Crazyflie can be armed...arming!') + safety_check(sup) + sup.send_arming_request(True) + time.sleep(1) + + safety_check(sup) + if sup.can_fly: + print('The Crazyflie can fly...taking off!') + commander.takeoff(1.0, 2.0) + time.sleep(3) + + safety_check(sup) + if sup.is_flying: + print('The Crazyflie is flying...landing!') + commander.land(0.0, 2.0) + time.sleep(3) + + safety_check(sup) + + except Exception as e: + print(f'Safety check failed: {e}') + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + time.sleep(1) + supervisor = scf.cf.supervisor + time.sleep(1) + run_sequence(scf, supervisor) diff --git a/examples/supervisor/reading_supervisor.py b/examples/supervisor/reading_supervisor.py new file mode 100644 index 000000000..edbe68e5f --- /dev/null +++ b/examples/supervisor/reading_supervisor.py @@ -0,0 +1,62 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2025 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Simple, non-flying example demonstrating how to read the Crazyflie's state +through the supervisor. Hold the Crazyflie in your hand and tilt it upside +down to observe the state changes. Once the tilt exceeds ~90°, the can_fly +state becomes False and the is_tumbled state becomes True. +""" +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper + +logging.basicConfig(level=logging.INFO) + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + time.sleep(1) + supervisor = scf.cf.supervisor + time.sleep(1) + try: + while True: + print('==============================================================================') + print(f'Can fly: {supervisor.can_fly}') + print(f'Is tumbled: {supervisor.is_tumbled}') + print(f'Bitfield: {supervisor.read_bitfield()}') + print(f'State list: {supervisor.read_state_list()}') + print('==============================================================================') + time.sleep(0.5) + + except KeyboardInterrupt: + print('Script terminated') diff --git a/examples/swarm/asynchronized_swarm.py b/examples/swarm/asynchronized_swarm.py index 3d9b3d591..5c2deea32 100644 --- a/examples/swarm/asynchronized_swarm.py +++ b/examples/swarm/asynchronized_swarm.py @@ -69,7 +69,7 @@ def wait_for_param_download(scf): def arm(scf): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/swarm/hl_commander_swarm.py b/examples/swarm/hl_commander_swarm.py index a3f4ddf27..ace7c05dc 100644 --- a/examples/swarm/hl_commander_swarm.py +++ b/examples/swarm/hl_commander_swarm.py @@ -45,7 +45,7 @@ def activate_mellinger_controller(scf, use_mellinger): def arm(scf): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/swarm/leader_follower.py b/examples/swarm/leader_follower.py index 552133af3..6d4802151 100644 --- a/examples/swarm/leader_follower.py +++ b/examples/swarm/leader_follower.py @@ -81,7 +81,7 @@ def wait_for_param_download(scf): def arm(scf): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/swarm/swarm_sequence.py b/examples/swarm/swarm_sequence.py index 132dfb81c..b3f369ba6 100644 --- a/examples/swarm/swarm_sequence.py +++ b/examples/swarm/swarm_sequence.py @@ -173,7 +173,7 @@ def wait_for_param_download(scf): def arm(scf): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) diff --git a/examples/swarm/swarm_sequence_circle.py b/examples/swarm/swarm_sequence_circle.py index 90ecd7bbb..181fc6b7a 100644 --- a/examples/swarm/swarm_sequence_circle.py +++ b/examples/swarm/swarm_sequence_circle.py @@ -89,7 +89,7 @@ def run_sequence(scf, params): cf = scf.cf # Arm the Crazyflie - cf.platform.send_arming_request(True) + cf.supervisor.send_arming_request(True) time.sleep(1.0) # Number of setpoints sent per second diff --git a/examples/swarm/synchronized_sequence.py b/examples/swarm/synchronized_sequence.py index f6483f4b4..f2201e84f 100755 --- a/examples/swarm/synchronized_sequence.py +++ b/examples/swarm/synchronized_sequence.py @@ -112,7 +112,7 @@ def activate_mellinger_controller(scf, use_mellinger): def arm(scf): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0)