Merge pull request #1348 from bitcraze/evoggy/supervisor-crash-recovery

Added crashed state and the ability to recover from it

src/modules/interface/supervisor.h

@@ -110,9 +110,26 @@ bool supervisorCanArm();
 bool supervisorIsArmed();
 
 /**
- * @brief Query if the system is locked (due to a crash)
+ * @brief Query if the system is locked (due to an emergency stop or watchdog timeout)
  *
  * @return true
  * @return false
  */
 bool supervisorIsLocked();
+
+/**
+ * @brief Query if the system is crashed
+ *
+ * @return true
+ * @return false
+ */
+bool supervisorIsCrashed();
+
+/**
+ * @brief Request the system to be recover if crashed.
+ *
+ * @param doRecover true - request recovery. false - request crashed.
+ * @return true   The request was granted
+ * @return false  The request could not be granted
+ */
+bool supervisorRequestCrashRecovery(const bool doRecover);

src/modules/interface/supervisor_state_machine.h

@@ -37,6 +37,7 @@ typedef enum {
     supervisorStateWarningLevelOut,
     supervisorStateExceptFreeFall,
     supervisorStateLocked,
+    supervisorStateCrashed,
     supervisorState_NrOfStates,
 } supervisorState_t;
 
@@ -48,6 +49,7 @@ typedef enum {
   supervisorConditionCommanderWdtWarning,
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
+  supervisorConditionIsCrashed,
   supervisorCondition_NrOfConditions,
 } supervisorConditions_t;
 
@@ -61,6 +63,7 @@ typedef uint32_t supervisorConditionBits_t;
 #define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning)
 #define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout)
 #define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop)
+#define SUPERVISOR_CB_CRASHED (1 << supervisorConditionIsCrashed)
 
 
 // Enum that is used to describe how to combine the bits in the required field

src/modules/src/platformservice.c

@@ -54,6 +54,7 @@ typedef enum {
 typedef enum {
   setContinuousWave  = 0x00,
   armSystem          = 0x01,
+  recoverSystem     = 0x02, 
 } PlatformCommand;
 
 typedef enum {
@@ -135,6 +136,14 @@ static void platformCommandProcess(CRTPPacket *p)
       p->size = 2;
       break;
     }
+    case recoverSystem:
+    {
+      const bool success = supervisorRequestCrashRecovery(true);
+      data[0] = success;
+      data[1] = !supervisorIsCrashed();
+      p->size = 2;
+      break;
+    }    
     default:
       break;
   }

src/modules/src/supervisor.c

@@ -66,6 +66,7 @@ typedef struct {
   bool isFlying;
   bool isTumbled;
   bool isArmingActivated;
+  bool isCrashed;
   uint16_t infoBitfield;
   uint8_t paramEmergencyStop;
 
@@ -115,6 +116,25 @@ bool supervisorIsLocked() {
   return supervisorStateLocked == supervisorMem.state;
 }
 
+bool supervisorIsCrashed() {
+  return supervisorMem.isCrashed;
+}
+
+bool supervisorRequestCrashRecovery(const bool doRecovery) {
+
+  if (doRecovery && !supervisorIsCrashed()) {
+    return true;
+  } else if (doRecovery && supervisorIsCrashed() && !supervisorIsTumbled()) {
+    supervisorMem.isCrashed = false;
+    return true;
+  } else if (!doRecovery) {
+    supervisorMem.isCrashed = true;
+    return true;
+  }
+
+  return false;
+}
+
 bool supervisorRequestArming(const bool doArm) {
   if (doArm == supervisorMem.isArmingActivated) {
     return true;
@@ -226,6 +246,11 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
     DEBUG_PRINT("Locked, reboot required\n");
   }
 
+  if (newState == supervisorStateCrashed) {
+    DEBUG_PRINT("Crashed, recovery required\n");
+    supervisorRequestCrashRecovery(false);
+  }
+
   if ((previousState == supervisorStateNotInitialized || previousState == supervisorStateReadyToFly || previousState == supervisorStateFlying) &&
       newState != supervisorStateReadyToFly && newState != supervisorStateFlying) {
     DEBUG_PRINT("Can not fly\n");
@@ -282,6 +307,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
     conditions |= SUPERVISOR_CB_EMERGENCY_STOP;
   }
 
+  if (supervisorIsCrashed()) {
+    conditions |= SUPERVISOR_CB_CRASHED;
+  }
+
   return conditions;
 }
 
@@ -312,6 +341,9 @@ static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t
   if (supervisorStateLocked == this->state) {
     this->infoBitfield |= 0x0040;
   }
+  if (this->isCrashed) {
+    this->infoBitfield |= 0x0080;
+  }
 }
 
 void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) {

src/modules/src/supervisor_state_machine.c

@@ -47,6 +47,7 @@ static const char* const stateNames[] = {
   "Warning, level out",
   "Exception, free fall",
   "Locked",
+  "Crashed",
 };
 static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates);
 
@@ -57,6 +58,7 @@ static const char* const conditionNames[] = {
   "commanderWdtWarning",
   "commanderWdtTimeout",
   "emergencyStop",
+  "isCrashed",
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
@@ -162,7 +164,16 @@ static SupervisorStateTransition_t transitionsFlying[] = {
   {
     .newState = supervisorStateExceptFreeFall,
 
-    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_EMERGENCY_STOP,
+    .negatedTriggers = SUPERVISOR_CB_ARMED,
+    .triggerCombiner = supervisorAny,
+
+    .blockerCombiner = supervisorNever,
+  },
+  {
+    .newState = supervisorStateCrashed,
+
+    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -249,6 +260,28 @@ static SupervisorStateTransition_t transitionsLocked[] = {
   },
 };
 
+
+static SupervisorStateTransition_t transitionsTumbled[] = {
+  {
+    .newState = supervisorStatePreFlChecksNotPassed,
+
+    .triggers = SUPERVISOR_CB_NONE,
+    .negatedTriggers = SUPERVISOR_CB_CRASHED | SUPERVISOR_CB_IS_TUMBLED,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever
+  },
+  {
+    .newState = supervisorStateLocked,
+
+    .triggers = SUPERVISOR_CB_EMERGENCY_STOP,
+    .negatedTriggers = SUPERVISOR_CB_NONE,
+    .triggerCombiner = supervisorAll,
+
+    .blockerCombiner = supervisorNever
+  },
+};
+
 SupervisorStateTransitionList_t transitionLists[] = {
   {SUPERVISOR_TRANSITION_ENTRY(transitionsNotInitialized)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksNotPassed)},
@@ -260,6 +293,7 @@ SupervisorStateTransitionList_t transitionLists[] = {
   {SUPERVISOR_TRANSITION_ENTRY(transitionsWarningLevelOut)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptFreeFall)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsLocked)},
+  {SUPERVISOR_TRANSITION_ENTRY(transitionsTumbled)},
 };
 static_assert(sizeof(transitionLists) / sizeof(transitionLists[0]) == supervisorState_NrOfStates);