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,46 @@
+---
+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       +------------|-------->+      Warning      +
++                   +<-----------|---------+     Level out     +
++--------+-------+--+            |         +---------+---------+
+         |       |               |                   |
+         V       +---------------|-----+             V
++-------------------+            |     |   +-------------------+
++      Landed       +------------+     +-->+     Exception     +
++                   |                      +     Free fall     +
++--------+----------+                      +---------+---------+
+                                                     |
+                                                     |
+                                      +--------------+
+                                      |
+                                      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/interface/supervisor_state_machine.h

@@ -0,0 +1,80 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+typedef enum {
+    supervisorStatePreFlChecksNotPassed = 0,
+    supervisorStatePreFlChecksPassed,
+    supervisorStateReadyToFly,
+    supervisorStateFlying,
+    supervisorStateLanded,
+    supervisorStateReset,
+    supervisorStateWarningLevelOut,
+    supervisorStateExceptFreeFall,
+    supervisorStateLocked,
+    supervisorState_NrOfStates,
+} supervisorState_t;
+
+// Conditions supported by the supervisor
+enum {
+  supervisorConditionArmed = 0,
+  supervisorConditionChargerConnected,
+  supervisorConditionIsFlying,
+  supervisorConditionIsTumbled,
+  supervisorConditionCommanderWdtWarning,
+  supervisorConditionCommanderWdtTimeout,
+  supervisorConditionEmergencyStop,
+};
+
+typedef uint32_t supervisorConditionBits_t;
+
+// Condition bit definitions
+#define SUPERVISOR_CB_NONE (0)
+#define SUPERVISOR_CB_ARMED (1 << supervisorConditionArmed)
+#define SUPERVISOR_CB_CHARGER_CONNECTED (1 << supervisorConditionChargerConnected)
+#define SUPERVISOR_CB_IS_FLYING (1 << supervisorConditionIsFlying)
+#define SUPERVISOR_CB_IS_TUMBLED (1 << supervisorConditionIsTumbled)
+#define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning)
+#define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout)
+#define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop)
+
+
+typedef struct {
+  supervisorState_t newState;
+  supervisorConditionBits_t mustBeSet;
+  supervisorConditionBits_t mustNotBeSet;
+} SupervisorStateTransition_t;
+
+typedef struct {
+  SupervisorStateTransition_t* transitionList;
+  int length;
+} SupervisorStateTransitionList_t;
+
+// Macro used when defining SupervisorStateTransitionLists
+#define SUPERVISOR_TRANSITION_ENTRY(TRANSITION_DEF) .transitionList=TRANSITION_DEF, .length=(sizeof(TRANSITION_DEF) / sizeof(SupervisorStateTransition_t))
+
+supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBits_t conditions);

src/modules/src/Kbuild

@@ -41,6 +41,7 @@ obj-y += sound_cf2.o
 obj-y += stabilizer.o
 obj-y += static_mem.o
 obj-y += supervisor.o
+obj-y += supervisor_state_machine.o
 obj-y += sysload.o
 obj-y += system.o
 obj-$(CONFIG_DECK_LOCO) += tdoaEngineInstance.o

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)