diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h index 72c5a8bf7e..164284d572 100644 --- a/src/modules/interface/supervisor_state_machine.h +++ b/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 diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index c274c0219e..66649ca353 100644 --- a/src/modules/src/supervisor.c +++ b/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) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index f12c8cbf41..2ccb2c7e65 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/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[] = { diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h index 427bc39a47..e85abfb7de 100644 --- a/src/platform/interface/platform_defaults.h +++ b/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