From 971eeb201670ef0a9402bcd755a9db565c6f4209 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 20 Feb 2024 13:18:51 +0100 Subject: [PATCH] Added crashed state and the ability to recover from it --- src/modules/interface/supervisor.h | 19 +++++++++- .../interface/supervisor_state_machine.h | 3 ++ src/modules/src/platformservice.c | 9 +++++ src/modules/src/supervisor.c | 32 +++++++++++++++++ src/modules/src/supervisor_state_machine.c | 36 ++++++++++++++++++- 5 files changed, 97 insertions(+), 2 deletions(-) diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index 48de77d8ed..46e2be9b4c 100644 --- a/src/modules/interface/supervisor.h +++ b/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); \ No newline at end of file diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h index 4c62a9d590..72c5a8bf7e 100644 --- a/src/modules/interface/supervisor_state_machine.h +++ b/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 diff --git a/src/modules/src/platformservice.c b/src/modules/src/platformservice.c index 28a3846c86..9a9972a852 100644 --- a/src/modules/src/platformservice.c +++ b/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; } diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 2e209829ab..c274c0219e 100644 --- a/src/modules/src/supervisor.c +++ b/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) { diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index b3cd69ba75..f12c8cbf41 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/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);