Extended supervisor

docs/functional-areas/supervisor/index.md

@@ -0,0 +1,9 @@
+---
+title: The supervisor
+page_id: supervisor_index
+---
+
+The purpose of the supervisor is to monitor the system and its state. Depending on the situation, the supervisor
+can enable/disable functionality as well as take action to protect the system or humans close by.
+
+{% sub_page_menu %}

docs/functional-areas/supervisor/states.md

@@ -0,0 +1,53 @@
+---
+title: Supervisor states
+page_id: supervisor_states
+---
+{% ditaa --alt "Supervisor states" %}
+     Boot|
+         V
++-------------------+
++ Pre-flight checks +<------------+
++    not passed     +<--+         |
++-------+-----------+   |         |
+        | ^             |         |
+        V |             |         |
++---------+---------+   |         |
++ Pre-flight checks +   |         |
++      passed       +   |         |
++-------+-----------+   |         |
+        | ^             | +-------+---------+
+  Arming| |             | +      Reset      +
+        V |             | +                 +
++---------+---------+   | +-----------------+
++   Ready to fly    +---+        ^ ^
++                   +            | |
++--------+----------+            | |
+         |                       | |
+         V                       | |
++-------------------+            | |       +-------------------+
++      Flying       +------------|-|------>+     Exception     +
++                   +------------|-|---+   +     Free fall     +--+
++--------+----------+            | |   |   +-------------------+  |
+         |                       | |   |                          |
+         V                       | |   |                          |
++-------------------+            | |   |   +-------------------+  |
++      Landed       +------------+ |   +-->+     Exception     +  |
++                   |              |       +     Soft fall     +  |
++--------+----------+              |       +---------+---------+  |
+         |                         |                 |            |
+         |                         |                 |  +---------+
+         |                         |                 |  |
+         |                         |                 V  V
+         +---------------+         |       +-------------------+
+                         |         +-------+    Platform not   +
+                         |                 +      moving       +
+                         |                 +---------+---------+
+                         |                           |
+                         |            +--------------+
+                         |            |
+                         V            V
+                      +-------------------+
+                      +       Lock        +
+                      +                   +
+                      +-------------------+
+{% endditaa %}

src/modules/interface/commander.h

@@ -32,9 +32,6 @@
 
 #define DEFAULT_YAW_MODE  XMODE
 
-#define COMMANDER_WDT_TIMEOUT_STABILIZE  M2T(500)
-#define COMMANDER_WDT_TIMEOUT_SHUTDOWN   M2T(2000)
-
 #define COMMANDER_PRIORITY_DISABLE   0
 // Keep a macro for lowest non-disabled priority, regardless of source, in case
 // some day there is a priority lower than the high-level commander.

src/modules/interface/crtp_localization_service.h

@@ -78,4 +78,24 @@ void locSrvSendRangeFloat(uint8_t id, float range);
 void locSrvSendLighthouseAngle(int baseStation, pulseProcessorResult_t* angles);
 #endif
 
+/**
+ * @brief Check if there is a request for emergency stop.
+ *
+ * @return true   Emergency stop requested
+ * @return false  Emergency stop not requested
+ */
+bool locSrvIsEmergencyStopRequested();
+
+/**
+ * @brief Reset any emergency stop request
+ */
+void locSrvResetEmergencyStopRequest();
+
+/**
+ * @brief Get the time for when the latest emergency stop notification was received.
+ *
+ * @return uint32_t  The system tick when the latest notification was received. 0 if no notification has been received.
+ */
+uint32_t locSrvGetEmergencyStopWatchdogNotificationTick();
+
 #endif /* _CRTP_LOCALIZATION_SERVICE_H_ */

src/modules/interface/stabilizer.h

@@ -31,7 +31,6 @@
 
 #include "estimator.h"
 
-#define EMERGENCY_STOP_TIMEOUT_DISABLED (-1)
 /**
  * Initialize the stabilizer subsystem and launch the stabilizer loop task.
  * The stabilizer loop task will wait on systemWaitStart() before running.
@@ -45,23 +44,4 @@ void stabilizerInit(StateEstimatorType estimator);
  */
 bool stabilizerTest(void);
 
-/**
- * Enable emergency stop, will shut-off energy to the motors.
- */
-void stabilizerSetEmergencyStop();
-
-/**
- * Disable emergency stop, will enable energy to the motors.
- */
-void stabilizerResetEmergencyStop();
-
-/**
- * Restart the countdown until emergercy stop will be enabled.
- *
- * @param timeout Timeout in stabilizer loop tick. The stabilizer loop rate is
- *                RATE_MAIN_LOOP.
- */
-void stabilizerSetEmergencyStopTimeout(int timeout);
-
-
 #endif /* STABILIZER_H_ */

src/modules/interface/supervisor.h

@@ -28,7 +28,10 @@
 
 #include "stabilizer_types.h"
 
-void supervisorUpdate(const sensorData_t *data);
+void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint);
+
+void supervisorOverrideSetpoint(setpoint_t* setpoint);
+bool supervisorAreMotorsAllowedToRun();
 
 bool supervisorCanFly(void);
 bool supervisorIsFlying(void);

src/modules/src/commander.c

@@ -102,23 +102,7 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
 {
   xQueuePeek(setpointQueue, setpoint, 0);
   lastUpdate = setpoint->timestamp;
-  uint32_t currentTime = xTaskGetTickCount();
-
-  if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_SHUTDOWN) {
-    memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint));
-  } else if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_STABILIZE) {
-    xQueueOverwrite(priorityQueue, &priorityDisable);
-    // Leveling ...
-    setpoint->mode.x = modeDisable;
-    setpoint->mode.y = modeDisable;
-    setpoint->mode.roll = modeAbs;
-    setpoint->mode.pitch = modeAbs;
-    setpoint->mode.yaw = modeVelocity;
-    setpoint->attitude.roll = 0;
-    setpoint->attitude.pitch = 0;
-    setpoint->attitudeRate.yaw = 0;
-    // Keep Z as it is
-  }
+
   // This copying is not strictly necessary because stabilizer.c already keeps
   // a static state_t containing the most recent state estimate. However, it is
   // not accessible by the public interface.

src/modules/src/crtp_localization_service.c

@@ -59,7 +59,6 @@
 #define NBR_OF_SENSOR_DIFFS_IN_PACKET   3
 #define NBR_OF_BASESTATIONS   2
 #define NBR_OF_
-#define DEFAULT_EMERGENCY_STOP_TIMEOUT (1 * RATE_MAIN_LOOP)
 
 typedef enum
 {
@@ -130,6 +129,9 @@ static void extPositionHandler(CRTPPacket* pk);
 static void genericLocHandle(CRTPPacket* pk);
 static void extPositionPackedHandler(CRTPPacket* pk);
 
+static bool isEmergencyStopRequested = false;
+static uint32_t emergencyStopWatchdogNotificationTick = 0;
+
 void locSrvInit()
 {
   if (isInit) {
@@ -294,10 +296,10 @@ static void genericLocHandle(CRTPPacket* pk)
       lpsShortLppPacketHandler(pk);
       break;
     case EMERGENCY_STOP:
-      stabilizerSetEmergencyStop();
+      isEmergencyStopRequested = true;
       break;
     case EMERGENCY_STOP_WATCHDOG:
-      stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT);
+      emergencyStopWatchdogNotificationTick = xTaskGetTickCount();
       break;
     case EXT_POSE:
       extPoseHandler(pk);
@@ -390,6 +392,18 @@ void locSrvSendLighthouseAngle(int baseStation, pulseProcessorResult_t* angles)
 }
 #endif
 
+bool locSrvIsEmergencyStopRequested() {
+  return isEmergencyStopRequested;
+}
+
+void locSrvResetEmergencyStopRequest() {
+  isEmergencyStopRequested = false;
+}
+
+uint32_t locSrvGetEmergencyStopWatchdogNotificationTick() {
+  return emergencyStopWatchdogNotificationTick;
+}
+
 // This logging group is deprecated (removed after August 2023)
 LOG_GROUP_START(ext_pos)
   LOG_ADD(LOG_FLOAT, X, &ext_pos.x)

src/modules/src/stabilizer.c

@@ -58,8 +58,6 @@
 #include "rateSupervisor.h"
 
 static bool isInit;
-static bool emergencyStop = false;
-static int emergencyStopTimeout = EMERGENCY_STOP_TIMEOUT_DISABLED;
 
 static uint32_t inToOutLatency;
 
@@ -204,17 +202,6 @@ bool stabilizerTest(void)
   return pass;
 }
 
-static void checkEmergencyStopTimeout()
-{
-  if (emergencyStopTimeout >= 0) {
-    emergencyStopTimeout -= 1;
-
-    if (emergencyStopTimeout == 0) {
-      emergencyStop = true;
-    }
-  }
-}
-
 static void batteryCompensation(const motors_thrust_uncapped_t* motorThrustUncapped, motors_thrust_uncapped_t* motorThrustBatCompUncapped)
 {
   float supplyVoltage = pmGetBatteryVoltage();
@@ -233,6 +220,25 @@ static void setMotorRatios(const motors_thrust_pwm_t* motorPwm)
   motorsSetRatio(MOTOR_M4, motorPwm->motors.m4);
 }
 
+static void updateStateEstimatorAndControllerTypes() {
+  if (stateEstimatorGetType() != estimatorType) {
+    stateEstimatorSwitchTo(estimatorType);
+    estimatorType = stateEstimatorGetType();
+  }
+
+  if (controllerGetType() != controllerType) {
+    controllerInit(controllerType);
+    controllerType = controllerGetType();
+  }
+}
+
+static void controlMotors(const control_t* control) {
+  powerDistribution(control, &motorThrustUncapped);
+  batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped);
+  powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm);
+  setMotorRatios(&motorPwm);
+}
+
 /* The stabilizer loop runs at 1kHz. It is the
  * responsibility of the different functions to run slower by skipping call
  * (ie. returning without modifying the output structure).
@@ -270,48 +276,40 @@ static void stabilizerTask(void* param)
     if (healthShallWeRunTest()) {
       healthRunTests(&sensorData);
     } else {
-      // allow to update estimator dynamically
-      if (stateEstimatorGetType() != estimatorType) {
-        stateEstimatorSwitchTo(estimatorType);
-        estimatorType = stateEstimatorGetType();
-      }
-      // allow to update controller dynamically
-      if (controllerGetType() != controllerType) {
-        controllerInit(controllerType);
-        controllerType = controllerGetType();
-      }
+      updateStateEstimatorAndControllerTypes();
 
       stateEstimator(&state, stabilizerStep);
-      compressState();
 
       if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, stabilizerStep)) {
         commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL);
       }
-
       commanderGetSetpoint(&setpoint, &state);
-      compressSetpoint();
 
-      collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep);
+      // Critical for safety, be careful if you modify this code!
+      // Let the supervisor update it's view of the current situation
+      supervisorUpdate(&sensorData, &setpoint);
 
-      controller(&control, &setpoint, &sensorData, &state, stabilizerStep);
+      // Let the collision avoidance module modify the setpoint, if needed
+      collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep);
 
-      checkEmergencyStopTimeout();
+      // Critical for safety, be careful if you modify this code!
+      // Let the supervisor modify the setpoint to handle exceptional conditions
+      supervisorOverrideSetpoint(&setpoint);
 
-      //
-      // The supervisor module keeps track of Crazyflie state such as if
-      // we are ok to fly, or if the Crazyflie is in flight.
-      //
-      supervisorUpdate(&sensorData);
+      controller(&control, &setpoint, &sensorData, &state, stabilizerStep);
 
-      if (emergencyStop || (systemIsArmed() == false)) {
-        motorsStop();
+      // Critical for safety, be careful if you modify this code!
+      // The supervisor will already set thrust to 0 in the setpoint if needed, but to be extra sure prevent motors from running.
+      if (supervisorAreMotorsAllowedToRun()) {
+        controlMotors(&control);
       } else {
-        powerDistribution(&control, &motorThrustUncapped);
-        batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped);
-        powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm);
-        setMotorRatios(&motorPwm);
+        motorsStop();
       }
 
+      // Compute compressed log formats
+      compressState();
+      compressSetpoint();
+
 #ifdef CONFIG_DECK_USD
       // Log data to uSD card if configured
       if (usddeckLoggingEnabled()
@@ -337,25 +335,9 @@ static void stabilizerTask(void* param)
   }
 }
 
-void stabilizerSetEmergencyStop()
-{
-  emergencyStop = true;
-}
-
-void stabilizerResetEmergencyStop()
-{
-  emergencyStop = false;
-}
-
-void stabilizerSetEmergencyStopTimeout(int timeout)
-{
-  emergencyStop = false;
-  emergencyStopTimeout = timeout;
-}
-
 /**
  * Parameters to set the estimator and controller type
- * for the stabilizer module, or to do an emergency stop
+ * for the stabilizer module
  */
 PARAM_GROUP_START(stabilizer)
 /**
@@ -368,10 +350,6 @@ PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType)
  * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0)
  */
 PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType)
-/**
- * @brief If set to nonzero will turn off power
- */
-PARAM_ADD_CORE(PARAM_UINT8, stop, &emergencyStop)
 PARAM_GROUP_STOP(stabilizer)