Merge pull request #1285 from bitcraze/krichardsson/supervisor-update

Remove "charger connected" from supervisor

docs/functional-areas/supervisor/states.md

@@ -8,6 +8,12 @@ page_id: supervisor_states
      Boot|
          V
 +-------------------+
++  Not initialized  +
++                   +
++--------+----------+
+         |
+         V
++-------------------+
 + Pre-flight checks +<------------+
 +    not passed     +<--+         |
 +-------+-----------+   |         |

src/modules/interface/supervisor_state_machine.h

@@ -27,7 +27,8 @@
 #include <stdint.h>
 
 typedef enum {
-    supervisorStatePreFlChecksNotPassed = 0,
+    supervisorStateNotInitialized = 0,
+    supervisorStatePreFlChecksNotPassed,
     supervisorStatePreFlChecksPassed,
     supervisorStateReadyToFly,
     supervisorStateFlying,
@@ -40,22 +41,21 @@ typedef enum {
 } supervisorState_t;
 
 // Conditions supported by the supervisor
-enum {
+typedef enum {
   supervisorConditionArmed = 0,
-  supervisorConditionChargerConnected,
   supervisorConditionIsFlying,
   supervisorConditionIsTumbled,
   supervisorConditionCommanderWdtWarning,
   supervisorConditionCommanderWdtTimeout,
   supervisorConditionEmergencyStop,
-};
+  supervisorCondition_NrOfConditions,
+} supervisorConditions_t;
 
 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)
@@ -93,3 +93,6 @@ typedef struct {
 #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);
+
+const char* supervisorGetStateName(const supervisorState_t currentState);
+const char* supervisorGetConditionName(const supervisorState_t condition);

src/modules/src/supervisor.c

@@ -27,12 +27,15 @@
 #include <math.h>
 #include <stdbool.h>
 #include <stdlib.h>
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
 
 #include "log.h"
 #include "param.h"
 #include "motors.h"
 #include "power_distribution.h"
-#include "pm.h"
 #include "supervisor.h"
 #include "supervisor_state_machine.h"
 #include "platform_defaults.h"
@@ -67,12 +70,17 @@ typedef struct {
   uint32_t latestThrustTick;
 
   supervisorState_t state;
+
+  // Copy of latest conditions, for logging
+  supervisorConditionBits_t latestConditions;
+  uint8_t doinfodump;
 } SupervisorMem_t;
 
 static SupervisorMem_t supervisorMem;
 
 const static setpoint_t nullSetpoint;
 
+void infoDump(const SupervisorMem_t* this);
 
 bool supervisorCanFly() {
   return supervisorMem.canFly;
@@ -161,23 +169,19 @@ static void transitionActions(const supervisorState_t currentState, const superv
     DEBUG_PRINT("Locked, reboot required\n");
   }
 
-  if ((currentState == supervisorStateReadyToFly || currentState == supervisorStateFlying) &&
+  if ((currentState == supervisorStateNotInitialized || currentState == supervisorStateReadyToFly || currentState == supervisorStateFlying) &&
       newState != supervisorStateReadyToFly && newState != supervisorStateFlying) {
     DEBUG_PRINT("Can not fly\n");
   }
 }
 
-static supervisorConditionBits_t updateAndpopulateConditions(SupervisorMem_t* this, const sensorData_t *sensors, const setpoint_t* setpoint, const uint32_t currentTick) {
+static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* this, const sensorData_t *sensors, const setpoint_t* setpoint, const uint32_t currentTick) {
   supervisorConditionBits_t conditions = 0;
 
   if (systemIsArmed()) {
     conditions |= SUPERVISOR_CB_ARMED;
   }
 
-  if (pmIsChargerConnected()) {
-    conditions |= SUPERVISOR_CB_CHARGER_CONNECTED;
-  }
-
   const bool isFlying = isFlyingCheck(this, currentTick);
   if (isFlying) {
     conditions |= SUPERVISOR_CB_IS_FLYING;
@@ -225,14 +229,19 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s
   SupervisorMem_t* this = &supervisorMem;
   const uint32_t currentTick = xTaskGetTickCount();
 
-  const supervisorConditionBits_t conditions = updateAndpopulateConditions(this, sensors, setpoint, currentTick);
+  const supervisorConditionBits_t conditions = updateAndPopulateConditions(this, sensors, setpoint, currentTick);
   const supervisorState_t newState = supervisorStateUpdate(this->state, conditions);
   if (this->state != newState) {
     transitionActions(this->state, newState);
     this->state = newState;
   }
 
+  this->latestConditions = conditions;
   updateLogData(this, conditions);
+  if (this->doinfodump) {
+    this->doinfodump = 0;
+    infoDump(this);
+  }
 }
 
 void supervisorOverrideSetpoint(setpoint_t* setpoint) {
@@ -270,6 +279,23 @@ bool supervisorAreMotorsAllowedToRun() {
          (this->state == supervisorStateWarningLevelOut);
 }
 
+void infoDump(const SupervisorMem_t* this) {
+  DEBUG_PRINT("Supervisor info ---\n");
+  DEBUG_PRINT("State: %s\n", supervisorGetStateName(this->state));
+  DEBUG_PRINT("Conditions: (0x%lx)\n", this->latestConditions);
+  for (supervisorConditions_t condition = 0; condition < supervisorCondition_NrOfConditions; condition++) {
+    const supervisorConditionBits_t bit = 1 << condition;
+    int bitValue = 0;
+    if (this->latestConditions & bit) {
+      bitValue = 1;
+    }
+
+    DEBUG_PRINT("  %s (0x%lx): %u\n", supervisorGetConditionName(condition), bit, bitValue);
+  }
+}
+
+
+
 /**
  *  System loggable variables to check different system states.
  */
@@ -295,3 +321,14 @@ PARAM_GROUP_START(stabilizer)
  */
 PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.paramEmergencyStop)
 PARAM_GROUP_STOP(stabilizer)
+
+/**
+ * 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.
+ */
+PARAM_GROUP_START(superv)
+/**
+ * @brief Set to nonzero to dump information about the current supervisor state to the console log
+ */
+PARAM_ADD(PARAM_UINT8, infdmp, &supervisorMem.doinfodump)
+PARAM_GROUP_STOP(superv)

src/modules/src/supervisor_state_machine.c

@@ -25,6 +25,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include "supervisor_state_machine.h"
+#include "platform_defaults.h"
 #include "test_support.h"
 
 // #define DEBUG_ME
@@ -33,8 +34,10 @@
 #define DEBUG_MODULE "SUPST"
 #include "debug.h"
 #include "cfassert.h"
+#endif
 
 static const char* const stateNames[] = {
+  "Not initialized",
   "Pre-flight checks not passed",
   "Pre-flight checks passed",
   "Ready to fly",
@@ -46,17 +49,39 @@ static const char* const stateNames[] = {
   "Locked",
 };
 static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates);
-#endif
 
+static const char* const conditionNames[] = {
+  "armed",
+  "isFlying",
+  "isTumbled",
+  "commanderWdtWarning",
+  "commanderWdtTimeout",
+  "emergencyStop",
+};
+static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
+
+#if SUPERVISOR_TUMBLE_CHECK_ENABLE
+  #define SUPERVISOR_CB_CONF_IS_TUMBLED (SUPERVISOR_CB_IS_TUMBLED)
+#else
+  #define SUPERVISOR_CB_CONF_IS_TUMBLED (SUPERVISOR_CB_NONE)
+#endif
 
 // State transition definitions
+static SupervisorStateTransition_t transitionsNotInitialized[] = {
+  {
+    .newState = supervisorStatePreFlChecksNotPassed,
+    .triggerCombiner = supervisorAlways,
+    .blockerCombiner = supervisorNever,
+  }
+};
+
 static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = {
   {
     .newState = supervisorStatePreFlChecksPassed,
 
     .triggerCombiner = supervisorAlways,
 
-    .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .blockers = SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedBlockers = SUPERVISOR_CB_NONE,
     .blockerCombiner = supervisorAny,
   }
@@ -66,7 +91,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = {
   {
     .newState = supervisorStatePreFlChecksNotPassed,
 
-    .triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedTriggers = SUPERVISOR_CB_NONE,
     .triggerCombiner = supervisorAny,
 
@@ -79,7 +104,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = {
     .negatedTriggers = SUPERVISOR_CB_NONE,
     .triggerCombiner = supervisorAll,
 
-    .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .blockers = SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedBlockers = SUPERVISOR_CB_NONE,
     .blockerCombiner = supervisorAny,
   },
@@ -89,7 +114,7 @@ static SupervisorStateTransition_t transitionsReadyToFly[] = {
   {
     .newState = supervisorStatePreFlChecksNotPassed,
 
-    .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -110,7 +135,7 @@ static SupervisorStateTransition_t transitionsFlying[] = {
   {
     .newState = supervisorStateExceptFreeFall,
 
-    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -160,7 +185,7 @@ static SupervisorStateTransition_t transitionsWarningLevelOut[] = {
   {
     .newState = supervisorStateExceptFreeFall,
 
-    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -198,6 +223,7 @@ static SupervisorStateTransition_t transitionsLocked[] = {
 };
 
 SupervisorStateTransitionList_t transitionLists[] = {
+  {SUPERVISOR_TRANSITION_ENTRY(transitionsNotInitialized)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksNotPassed)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksPassed)},
   {SUPERVISOR_TRANSITION_ENTRY(transitionsReadyToFly)},
@@ -286,3 +312,11 @@ supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, co
 
   return newState;
 }
+
+const char* supervisorGetStateName(const supervisorState_t state) {
+  return stateNames[state];
+}
+
+const char* supervisorGetConditionName(const supervisorState_t condition) {
+  return conditionNames[condition];
+}