Merge pull request #1349 from bitcraze/rik/supervisor-landing-timeout

Start timeout after landing before disarming

src/modules/interface/supervisor_state_machine.h

@@ -50,6 +50,7 @@ typedef enum {
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
   supervisorConditionIsCrashed,
+  supervisorConditionLandingTimeout,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
 
@@ -64,6 +65,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_LANDING_TIMEOUT (1 << supervisorConditionLandingTimeout)
 
 
 // Enum that is used to describe how to combine the bits in the required field

src/modules/src/supervisor.c

@@ -61,6 +61,8 @@
   #define AUTO_ARMING 0
 #endif
 
+static uint16_t landingTimeoutDuration = LANDING_TIMEOUT_MS;
+
 typedef struct {
   bool canFly;
   bool isFlying;
@@ -79,6 +81,9 @@ typedef struct {
   // 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 landing event. 0=no landing event yet
+  uint32_t latestLandingTick;
+
   supervisorState_t state;
 
   // Copy of latest conditions, for logging
@@ -120,6 +125,19 @@ bool supervisorIsCrashed() {
   return supervisorMem.isCrashed;
 }
 
+static void supervisorSetLatestLandingTime(SupervisorMem_t* this, const uint32_t currentTick) {
+  this->latestLandingTick = currentTick;
+}
+
+bool supervisorIsLandingTimeout(SupervisorMem_t* this, const uint32_t currentTick) {
+  if (0 == this->latestLandingTick) {
+    return false;
+  }
+
+  const uint32_t landingTime = currentTick - this->latestLandingTick;
+  return landingTime > M2T(landingTimeoutDuration);
+}
+
 bool supervisorRequestCrashRecovery(const bool doRecovery) {
 
   if (doRecovery && !supervisorIsCrashed()) {
@@ -235,13 +253,21 @@ static bool checkEmergencyStopWatchdog(const uint32_t tick) {
   return isOk;
 }
 
-static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t previousState) {
+static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t previousState, const uint32_t currentTick) {
   const supervisorState_t newState = this->state;
 
   if (newState == supervisorStateReadyToFly) {
     DEBUG_PRINT("Ready to fly\n");
   }
 
+  if (newState == supervisorStateLanded) {
+    supervisorSetLatestLandingTime(this, currentTick);
+  }
+
+  if ((previousState == supervisorStateLanded) && (newState == supervisorStateReset)) {
+    DEBUG_PRINT("Landing timeout, disarming\n");
+  }
+
   if (newState == supervisorStateLocked) {
     DEBUG_PRINT("Locked, reboot required\n");
   }
@@ -252,13 +278,14 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   }
 
   if ((previousState == supervisorStateNotInitialized || previousState == supervisorStateReadyToFly || previousState == supervisorStateFlying) &&
-      newState != supervisorStateReadyToFly && newState != supervisorStateFlying) {
+      newState != supervisorStateReadyToFly && newState != supervisorStateFlying && newState != supervisorStateLanded) {
     DEBUG_PRINT("Can not fly\n");
   }
 
   if (newState != supervisorStateReadyToFly &&
       newState != supervisorStateFlying &&
-      newState != supervisorStateWarningLevelOut) {
+      newState != supervisorStateWarningLevelOut &&
+      newState != supervisorStateLanded) {
     supervisorRequestArming(false);
   }
 
@@ -311,6 +338,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_CRASHED;
   }
 
+  if (supervisorIsLandingTimeout(this, currentTick)) {
+    conditions |= SUPERVISOR_CB_LANDING_TIMEOUT;
+  }
+
   return conditions;
 }
 
@@ -359,7 +390,7 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s
   if (this->state != newState) {
     const supervisorState_t previousState = this->state;
     this->state = newState;
-    postTransitionActions(this, previousState);
+    postTransitionActions(this, previousState, currentTick);
   }
 
   this->latestConditions = conditions;
@@ -375,6 +406,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) {
   switch(this->state){
     case supervisorStateReadyToFly:
       // Fall through
+    case supervisorStateLanded:
+      // Fall through
     case supervisorStateFlying:
       // Do nothing
       break;
@@ -402,7 +435,8 @@ bool supervisorAreMotorsAllowedToRun() {
   SupervisorMem_t* this = &supervisorMem;
   return (this->state == supervisorStateReadyToFly) ||
          (this->state == supervisorStateFlying) ||
-         (this->state == supervisorStateWarningLevelOut);
+         (this->state == supervisorStateWarningLevelOut) ||
+         (this->state == supervisorStateLanded);
 }
 
 void infoDump(const SupervisorMem_t* this) {
@@ -494,4 +528,10 @@ PARAM_GROUP_START(supervisor)
  * @brief Set to nonzero to dump information about the current supervisor state to the console log
  */
 PARAM_ADD(PARAM_UINT8, infdmp, &supervisorMem.doinfodump)
+
+/**
+ * @brief Landing timeout duration (ms)
+ */
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, landedTimeout, &landingTimeoutDuration)
+
 PARAM_GROUP_STOP(supervisor)

src/modules/src/supervisor_state_machine.c

@@ -59,6 +59,7 @@ static const char* const conditionNames[] = {
   "commanderWdtTimeout",
   "emergencyStop",
   "isCrashed",
+  "landingTimeout",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
@@ -203,10 +204,21 @@ static SupervisorStateTransition_t transitionsLanded[] = {
   {
     .newState = supervisorStateReset,
 
-    .triggerCombiner = supervisorAlways,
+    .triggers = SUPERVISOR_CB_LANDING_TIMEOUT,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
 
     .blockerCombiner = supervisorNever,
   },
+  {
+    .newState = supervisorStateFlying,
+
+    .triggers = SUPERVISOR_CB_IS_FLYING,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever,
+  }
 };
 
 static SupervisorStateTransition_t transitionsReset[] = {

src/platform/interface/platform_defaults.h

@@ -125,6 +125,11 @@
     #define SUPERVISOR_TUMBLE_CHECK_ENABLE true
 #endif
 
+// Landing timeout before disarming
+#ifndef LANDING_TIMEOUT_MS
+    #define LANDING_TIMEOUT_MS 3000
+#endif
+
 
 // Health test parameters
 #ifndef HEALTH_BRUSHED_ON_PERIOD_MSEC