Merge pull request #1441 from bitcraze/rik/auto_disarm

Automatically disarm after N seconds without flying

src/modules/interface/supervisor_state_machine.h

@@ -50,6 +50,7 @@ typedef enum {
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
   supervisorConditionIsCrashed,
+  supervisorConditionPreflightTimeout,
   supervisorConditionLandingTimeout,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
@@ -65,6 +66,7 @@ typedef uint32_t supervisorConditionBits_t;
 #define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout)
 #define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop)
 #define SUPERVISOR_CB_CRASHED (1 << supervisorConditionIsCrashed)
+#define SUPERVISOR_CB_PREFLIGHT_TIMEOUT (1 << supervisorConditionPreflightTimeout)
 #define SUPERVISOR_CB_LANDING_TIMEOUT (1 << supervisorConditionLandingTimeout)
 
 

src/modules/src/supervisor.c

@@ -61,6 +61,7 @@
   #define AUTO_ARMING 0
 #endif
 
+static uint16_t preflightTimeoutDuration = PREFLIGHT_TIMEOUT_MS;
 static uint16_t landingTimeoutDuration = LANDING_TIMEOUT_MS;
 
 typedef struct {
@@ -72,15 +73,15 @@ typedef struct {
   uint16_t infoBitfield;
   uint8_t paramEmergencyStop;
 
-  // Deprecated, remove after 2024-06-01
-  int8_t deprecatedArmParam;
-
   // The time (in ticks) of the first tumble event. 0=no tumble
   uint32_t initialTumbleTick;
 
   // The time (in ticks) of the latest high thrust event. 0=no high thrust event yet
   uint32_t latestThrustTick;
 
+  // The time (in ticks) of the latest arming event. 0=no arming event yet
+  uint32_t latestArmingTick;
+
   // The time (in ticks) of the latest landing event. 0=no landing event yet
   uint32_t latestLandingTick;
 
@@ -114,7 +115,7 @@ bool supervisorCanArm() {
 }
 
 bool supervisorIsArmed() {
-  return supervisorMem.isArmingActivated || supervisorMem.deprecatedArmParam;
+  return supervisorMem.isArmingActivated;
 }
 
 bool supervisorIsLocked() {
@@ -125,10 +126,23 @@ bool supervisorIsCrashed() {
   return supervisorMem.isCrashed;
 }
 
+static void supervisorSetLatestArmingTime(SupervisorMem_t* this, const uint32_t currentTick) {
+  this->latestArmingTick = currentTick;
+}
+
 static void supervisorSetLatestLandingTime(SupervisorMem_t* this, const uint32_t currentTick) {
   this->latestLandingTick = currentTick;
 }
 
+bool supervisorIsPreflightTimeout(SupervisorMem_t *this, const uint32_t currentTick) {
+  if (supervisorStateReadyToFly != this->state) {
+    return false;
+  }
+
+  const uint32_t preflightTime = currentTick - this->latestArmingTick;
+  return preflightTime > M2T(preflightTimeoutDuration);
+}
+
 bool supervisorIsLandingTimeout(SupervisorMem_t* this, const uint32_t currentTick) {
   if (0 == this->latestLandingTick) {
     return false;
@@ -257,15 +271,21 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   const supervisorState_t newState = this->state;
 
   if (newState == supervisorStateReadyToFly) {
-    DEBUG_PRINT("Ready to fly\n");
+    if (!AUTO_ARMING){
+      DEBUG_PRINT("Ready to fly\n");
+    }
+    supervisorSetLatestArmingTime(this, currentTick);
   }
 
   if (newState == supervisorStateLanded) {
     supervisorSetLatestLandingTime(this, currentTick);
   }
 
-  if ((previousState == supervisorStateFlying || previousState == supervisorStateLanded) && (newState == supervisorStateReset)) {
-    DEBUG_PRINT("Disarming\n");
+  if (((previousState == supervisorStateFlying || previousState == supervisorStateLanded) && (newState == supervisorStateReset))
+       || (previousState == supervisorStateReadyToFly && newState == supervisorStatePreFlChecksPassed)) {
+    if (!AUTO_ARMING){
+      DEBUG_PRINT("Disarming\n");
+    }
   }
 
   if (newState == supervisorStateLocked) {
@@ -277,11 +297,6 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
     supervisorRequestCrashRecovery(false);
   }
 
-  if ((previousState == supervisorStateNotInitialized || previousState == supervisorStateReadyToFly || previousState == supervisorStateFlying) &&
-      newState != supervisorStateReadyToFly && newState != supervisorStateFlying && newState != supervisorStateLanded) {
-    DEBUG_PRINT("Can not fly\n");
-  }
-
   if (newState != supervisorStateReadyToFly &&
       newState != supervisorStateFlying &&
       newState != supervisorStateWarningLevelOut &&
@@ -290,7 +305,7 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   }
 
   // We do not require an arming action by the user, auto arm
-  if (AUTO_ARMING || this->deprecatedArmParam) {
+  if (AUTO_ARMING) {
     if (newState == supervisorStatePreFlChecksPassed) {
       supervisorRequestArming(true);
     }
@@ -343,6 +358,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_CRASHED;
   }
 
+  if (supervisorIsPreflightTimeout(this, currentTick)) {
+    conditions |= SUPERVISOR_CB_PREFLIGHT_TIMEOUT;
+  }
+
   if (supervisorIsLandingTimeout(this, currentTick)) {
     conditions |= SUPERVISOR_CB_LANDING_TIMEOUT;
   }
@@ -362,7 +381,7 @@ static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t
   if (supervisorIsArmed()) {
     this->infoBitfield |= 0x0002;
   }
-  if(AUTO_ARMING || this->deprecatedArmParam) {
+  if(AUTO_ARMING) {
     this->infoBitfield |= 0x0004;
   }
   if (this->canFly) {
@@ -494,17 +513,6 @@ PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.paramEmergencyStop)
 PARAM_GROUP_STOP(stabilizer)
 
 
-PARAM_GROUP_START(system)
-
-/**
- * @brief Set to nonzero to arm the system. A nonzero value enables the auto arm functionality
- *
- * Deprecated, will be removed after 2024-06-01. Use the CRTP `PlatformCommand` `armSystem` on the CRTP_PORT_PLATFORM port instead.
- */
-PARAM_ADD_CORE(PARAM_INT8, arm, &supervisorMem.deprecatedArmParam)
-PARAM_GROUP_STOP(system)
-
-
 /**
  * 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.
@@ -534,6 +542,12 @@ PARAM_GROUP_START(supervisor)
  */
 PARAM_ADD(PARAM_UINT8, infdmp, &supervisorMem.doinfodump)
 
+/**
+ * @brief Preflight timeout duration (ms)
+ * The time the system is allowed to be armed before it must be flying.
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, prefltTimeout, &preflightTimeoutDuration)
+
 /**
  * @brief Landing timeout duration (ms)
  */

src/modules/src/supervisor_state_machine.c

@@ -59,6 +59,7 @@ static const char* const conditionNames[] = {
   "commanderWdtTimeout",
   "emergencyStop",
   "isCrashed",
+  "preflightTimeout",
   "landingTimeout",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
@@ -138,7 +139,7 @@ static SupervisorStateTransition_t transitionsReadyToFly[] = {
   {
     .newState = supervisorStatePreFlChecksNotPassed,
 
-    .triggers = SUPERVISOR_CB_IS_TUMBLED,
+    .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_PREFLIGHT_TIMEOUT,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 

src/platform/interface/platform_defaults.h

@@ -147,6 +147,11 @@
     #define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_TIME 100
 #endif
 
+// Pre-flight disarming timeout
+#ifndef PREFLIGHT_TIMEOUT_MS
+    #define PREFLIGHT_TIMEOUT_MS 30000
+#endif
+
 // Landing timeout before disarming
 #ifndef LANDING_TIMEOUT_MS
     #define LANDING_TIMEOUT_MS 3000

src/platform/interface/platform_defaults_cf21bl.h

@@ -130,5 +130,4 @@
 #define PID_POS_VEL_Y_MAX 1.0f
 #define PID_POS_VEL_Z_MAX 1.0f
 
-// Manual arming, default idle thrust
 #define CONFIG_MOTORS_DEFAULT_IDLE_THRUST 7000