From aa3553c191f1c5756fab5a54f196a5149aeca4c2 Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 16 Sep 2025 16:52:29 +0200 Subject: [PATCH 1/7] Added SupervisorState util --- cflib/utils/supervisor_state.py | 137 ++++++++++++++++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 cflib/utils/supervisor_state.py diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py new file mode 100644 index 000000000..ad6130292 --- /dev/null +++ b/cflib/utils/supervisor_state.py @@ -0,0 +1,137 @@ +import threading + +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie + + +class SupervisorState: + STATES = [ + 'Can be armed', 'Is armed', '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): + if isinstance(crazyflie, SyncCrazyflie): + self.cf = crazyflie.cf + else: + self.cf = crazyflie + + def read_supervisor_state_bitfield(self): + """ + Reads 'supervisor.info' once from the Crazyflie. + Returns the value or None if timed out. + """ + value_holder = {'val': None} + event = threading.Event() + + def log_callback(timestamp, data, logconf): + value_holder['val'] = data['supervisor.info'] + event.set() + + def log_error(logconf, msg): + print(f'Error when logging {logconf.name}: {msg}') + event.set() + + logconf = LogConfig(name='SupervisorInfo', period_in_ms=100) + logconf.add_variable('supervisor.info', 'uint16_t') + + try: + self.cf.log.add_config(logconf) + except KeyError as e: + print('Could not add log config:', e) + return None + + logconf.data_received_cb.add_callback(log_callback) + logconf.error_cb.add_callback(log_error) + logconf.start() + + if event.wait(2.0): + bitfield = value_holder['val'] + else: + print('Timeout waiting for supervisor.info') + bitfield = None + + logconf.stop() + return bitfield + + def read_supervisor_state_list(self): + """ + Reads 'supervisor.info' once from the Crazyflie. + Returns the list of all active states. + """ + bitfield = self.read_supervisor_state_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 = auto arm - 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 + + # Individual state checks + def can_be_armed(self): + # Bit 0 + return bool(self.read_supervisor_state_bitfield() & (1 << 0)) + + def is_armed(self): + # Bit 1 + return bool(self.read_supervisor_state_bitfield() & (1 << 1)) + + def is_auto_armed(self): + # Bit 2 + return bool(self.read_supervisor_state_bitfield() & (1 << 2)) + + def can_fly(self): + # Bit 3 + return bool(self.read_supervisor_state_bitfield() & (1 << 3)) + + def is_flying(self): + # Bit 4 + return bool(self.read_supervisor_state_bitfield() & (1 << 4)) + + def is_tumbled(self): + # Bit 5 + return bool(self.read_supervisor_state_bitfield() & (1 << 5)) + + def is_locked(self): + # Bit 6 + return bool(self.read_supervisor_state_bitfield() & (1 << 6)) + + def is_crashed(self): + # Bit 7 + return bool(self.read_supervisor_state_bitfield() & (1 << 7)) + + def active_hl_control(self): + # Bit 8 + return bool(self.read_supervisor_state_bitfield() & (1 << 8)) + + def finished_hl_traj(self): + # Bit 9 + return bool(self.read_supervisor_state_bitfield() & (1 << 9)) + + def disabled_hl_control(self): + # Bit 10 + return bool(self.read_supervisor_state_bitfield() & (1 << 10)) From 01ccd7cfa1f453348fac0a4fabb22c1a671ce4b2 Mon Sep 17 00:00:00 2001 From: aris Date: Wed, 1 Oct 2025 16:55:24 +0200 Subject: [PATCH 2/7] reshaped the STATES list --- cflib/utils/supervisor_state.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py index ad6130292..76b99537c 100644 --- a/cflib/utils/supervisor_state.py +++ b/cflib/utils/supervisor_state.py @@ -6,9 +6,17 @@ class SupervisorState: STATES = [ - 'Can be armed', 'Is armed', 'Auto armed', 'Can fly', 'Is flying', - 'Is tumbled', 'Is locked', 'Is crashed', 'HL control is active', - 'Finished HL trajectory', 'HL control is disabled' + 'Can be armed', + 'Is armed', + '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): From 55195de614208484724c5231e2d80aaf5eda7a11 Mon Sep 17 00:00:00 2001 From: aris Date: Mon, 6 Oct 2025 14:44:34 +0200 Subject: [PATCH 3/7] Delete the logconfig after reading the SupervisorState --- cflib/utils/supervisor_state.py | 1 + 1 file changed, 1 insertion(+) diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py index 76b99537c..9f446ab3c 100644 --- a/cflib/utils/supervisor_state.py +++ b/cflib/utils/supervisor_state.py @@ -61,6 +61,7 @@ def log_error(logconf, msg): bitfield = None logconf.stop() + logconf.delete() return bitfield def read_supervisor_state_list(self): From 666c372a6bc351a9b51d15911b5dc575d7e577b5 Mon Sep 17 00:00:00 2001 From: aris Date: Mon, 6 Oct 2025 14:59:37 +0200 Subject: [PATCH 4/7] Added a simple Supervisor example --- examples/supervisor/reading_supervisor.py | 86 +++++++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 examples/supervisor/reading_supervisor.py diff --git a/examples/supervisor/reading_supervisor.py b/examples/supervisor/reading_supervisor.py new file mode 100644 index 000000000..256cf2239 --- /dev/null +++ b/examples/supervisor/reading_supervisor.py @@ -0,0 +1,86 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017-2018 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 of reading the state of the Crazyflie through the supervisor. + +Based on its state, the Crazyflie will arm (if it can be armed), take off +(if it can fly), and land (if it is flying). After each action, we call +the supervisor to check if the Crazyflie is crashed, locked or tumbled. +""" +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.utils import uri_helper +from cflib.utils.reset_estimator import reset_estimator +from cflib.utils.supervisor_state import SupervisorState + + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + + +def safety_check(sup: SupervisorState): + 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: SupervisorState): + commander = scf.cf.high_level_commander + + try: + if sup.can_be_armed(): + safety_check(sup) + scf.cf.platform.send_arming_request(True) + time.sleep(1) + + 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(e) + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + time.sleep(1) + supervisor = SupervisorState(scf) + reset_estimator(scf) + time.sleep(1) + run_sequence(scf, supervisor) From a358ddb2abea359aa9b95efc8ea90ce43c218098 Mon Sep 17 00:00:00 2001 From: aris Date: Mon, 6 Oct 2025 15:02:44 +0200 Subject: [PATCH 5/7] Added the Bitcraze logo --- cflib/utils/supervisor_state.py | 21 +++++++++++++++++++++ examples/supervisor/reading_supervisor.py | 2 +- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py index 9f446ab3c..f05f1a334 100644 --- a/cflib/utils/supervisor_state.py +++ b/cflib/utils/supervisor_state.py @@ -1,3 +1,24 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2020 Bitcraze AB +# +# 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, in version 3. +# +# 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 . import threading from cflib.crazyflie.log import LogConfig diff --git a/examples/supervisor/reading_supervisor.py b/examples/supervisor/reading_supervisor.py index 256cf2239..9fc9ba3c8 100644 --- a/examples/supervisor/reading_supervisor.py +++ b/examples/supervisor/reading_supervisor.py @@ -6,7 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2017-2018 Bitcraze AB +# Copyright (C) 2025 Bitcraze AB # # Crazyflie Nano Quadcopter Client # From fe74cabe5ac716d04388869b07cdf7ecdf9202c9 Mon Sep 17 00:00:00 2001 From: aris Date: Tue, 7 Oct 2025 15:03:29 +0200 Subject: [PATCH 6/7] Slightly reshaped supervisor files --- cflib/utils/supervisor_state.py | 31 +++++++++++++---------- examples/supervisor/reading_supervisor.py | 1 + 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py index f05f1a334..406bd3582 100644 --- a/cflib/utils/supervisor_state.py +++ b/cflib/utils/supervisor_state.py @@ -6,7 +6,7 @@ # | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2020 Bitcraze AB +# Copyright (C) 2025 Bitcraze AB # # 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 @@ -19,6 +19,11 @@ # # You should have received a copy of the GNU General Public License # along with this program. If not, see . +""" +This class is used to easily get the state of your Crazyflie by through +the supervisor. Check the `reading_supervisor.py` example in +examples/supervisor/ to better understand how it works. +""" import threading from cflib.crazyflie.log import LogConfig @@ -29,7 +34,7 @@ class SupervisorState: STATES = [ 'Can be armed', 'Is armed', - 'Auto armed', + 'Is auto armed', 'Can fly', 'Is flying', 'Is tumbled', @@ -100,17 +105,17 @@ def decode_bitfield(self, value): 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 = auto arm - 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 + * 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') diff --git a/examples/supervisor/reading_supervisor.py b/examples/supervisor/reading_supervisor.py index 9fc9ba3c8..6c1399cd1 100644 --- a/examples/supervisor/reading_supervisor.py +++ b/examples/supervisor/reading_supervisor.py @@ -56,6 +56,7 @@ def run_sequence(scf: SyncCrazyflie, sup: SupervisorState): try: if sup.can_be_armed(): + print('The Crazyflie can be armed...arming!') safety_check(sup) scf.cf.platform.send_arming_request(True) time.sleep(1) From a6b0cf26c6df96823891b8747a35bad208241854 Mon Sep 17 00:00:00 2001 From: aris Date: Sat, 25 Oct 2025 11:31:50 +0200 Subject: [PATCH 7/7] Moved LogConfig in the __init__ This avoids hitting the 255 log block limit --- cflib/utils/supervisor_state.py | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/cflib/utils/supervisor_state.py b/cflib/utils/supervisor_state.py index 406bd3582..3107e3950 100644 --- a/cflib/utils/supervisor_state.py +++ b/cflib/utils/supervisor_state.py @@ -51,6 +51,16 @@ def __init__(self, crazyflie): else: self.cf = crazyflie + self.logconf = LogConfig(name='SupervisorInfo', period_in_ms=100) + self.logconf.add_variable('supervisor.info', 'uint16_t') + self.cf.log.add_config(self.logconf) + + def close(self): + try: + self.logconf.delete() + except Exception as e: + print(f'Warning: failed to delete logconf: {e}') + def read_supervisor_state_bitfield(self): """ Reads 'supervisor.info' once from the Crazyflie. @@ -67,18 +77,9 @@ def log_error(logconf, msg): print(f'Error when logging {logconf.name}: {msg}') event.set() - logconf = LogConfig(name='SupervisorInfo', period_in_ms=100) - logconf.add_variable('supervisor.info', 'uint16_t') - - try: - self.cf.log.add_config(logconf) - except KeyError as e: - print('Could not add log config:', e) - return None - - logconf.data_received_cb.add_callback(log_callback) - logconf.error_cb.add_callback(log_error) - logconf.start() + self.logconf.data_received_cb.add_callback(log_callback) + self.logconf.error_cb.add_callback(log_error) + self.logconf.start() if event.wait(2.0): bitfield = value_holder['val'] @@ -86,8 +87,10 @@ def log_error(logconf, msg): print('Timeout waiting for supervisor.info') bitfield = None - logconf.stop() - logconf.delete() + self.logconf.stop() + self.logconf.data_received_cb.remove_callback(log_callback) + self.logconf.error_cb.remove_callback(log_error) + return bitfield def read_supervisor_state_list(self):