From 367572953fcc87158d284b0daca27a0416fb2bb7 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 21 Feb 2024 10:56:59 +0100 Subject: [PATCH 1/2] Start timeout after landing before disarming Previous behavior: The system would disarm (because of supervisor state reset) immediately upon landing. New behavior: Upon landing, the system will remain in a landed state, maintaining its armed status and readiness for takeoff. A configurable timeout mechanism is introduced, triggering the supervisor state reset and disarming the system after the specified duration. The timeout duration is a persistent parameter. --- .../interface/supervisor_state_machine.h | 2 + src/modules/src/supervisor.c | 47 +++++++++++++++++-- src/modules/src/supervisor_state_machine.c | 14 +++++- src/platform/interface/platform_defaults.h | 5 ++ 4 files changed, 62 insertions(+), 6 deletions(-) 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..804c46457a 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,18 @@ 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) { + DEBUG_PRINT("Landed, starting disarm timer\n"); + supervisorSetLatestLandingTime(this, currentTick); + } + if (newState == supervisorStateLocked) { DEBUG_PRINT("Locked, reboot required\n"); } @@ -252,13 +275,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 +335,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 +387,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 +403,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { switch(this->state){ case supervisorStateReadyToFly: // Fall through + case supervisorStateLanded: + // Fall through case supervisorStateFlying: // Do nothing break; @@ -402,7 +432,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 +525,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 From 76df43b79a675a9716e607ddea80e93404486c3a Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 21 Feb 2024 11:43:03 +0100 Subject: [PATCH 2/2] Notify user of landing timeout No longer notify for starting the timer. --- src/modules/src/supervisor.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 804c46457a..66649ca353 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -261,9 +261,12 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t } if (newState == supervisorStateLanded) { - DEBUG_PRINT("Landed, starting disarm timer\n"); supervisorSetLatestLandingTime(this, currentTick); } + + if ((previousState == supervisorStateLanded) && (newState == supervisorStateReset)) { + DEBUG_PRINT("Landing timeout, disarming\n"); + } if (newState == supervisorStateLocked) { DEBUG_PRINT("Locked, reboot required\n");