From 9a9337c037df428e7db19e3fce7cf8f82220b440 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 19 Apr 2023 10:54:34 +0200 Subject: [PATCH 01/30] Move emergency stop to supervisor --- .../interface/crtp_localization_service.h | 20 ++++ src/modules/interface/stabilizer.h | 20 ---- src/modules/interface/supervisor.h | 10 +- src/modules/src/crtp_localization_service.c | 20 +++- src/modules/src/stabilizer.c | 45 +-------- src/modules/src/supervisor.c | 91 +++++++++++++++---- 6 files changed, 123 insertions(+), 83 deletions(-) diff --git a/src/modules/interface/crtp_localization_service.h b/src/modules/interface/crtp_localization_service.h index d03719ec77..ef656482f6 100644 --- a/src/modules/interface/crtp_localization_service.h +++ b/src/modules/interface/crtp_localization_service.h @@ -78,4 +78,24 @@ void locSrvSendRangeFloat(uint8_t id, float range); void locSrvSendLighthouseAngle(int baseStation, pulseProcessorResult_t* angles); #endif +/** + * @brief Check if there is a request for emergency stop. + * + * @return true Emergency stop requested + * @return false Emergency stop not requested + */ +bool locSrvIsEmergencyStopRequested(); + +/** + * @brief Reset any emergency stop request + */ +void locSrvResetEmergencyStopRequest(); + +/** + * @brief Get the time for when the latest emergency stop notification was received. + * + * @return uint32_t The system tick when the latest notification was received. 0 if no notification has been received. + */ +uint32_t locSrvGetEmergencyStopWatchdogNotificationTick(); + #endif /* _CRTP_LOCALIZATION_SERVICE_H_ */ diff --git a/src/modules/interface/stabilizer.h b/src/modules/interface/stabilizer.h index d836b224c2..20d9503114 100644 --- a/src/modules/interface/stabilizer.h +++ b/src/modules/interface/stabilizer.h @@ -31,7 +31,6 @@ #include "estimator.h" -#define EMERGENCY_STOP_TIMEOUT_DISABLED (-1) /** * Initialize the stabilizer subsystem and launch the stabilizer loop task. * The stabilizer loop task will wait on systemWaitStart() before running. @@ -45,23 +44,4 @@ void stabilizerInit(StateEstimatorType estimator); */ bool stabilizerTest(void); -/** - * Enable emergency stop, will shut-off energy to the motors. - */ -void stabilizerSetEmergencyStop(); - -/** - * Disable emergency stop, will enable energy to the motors. - */ -void stabilizerResetEmergencyStop(); - -/** - * Restart the countdown until emergercy stop will be enabled. - * - * @param timeout Timeout in stabilizer loop tick. The stabilizer loop rate is - * RATE_MAIN_LOOP. - */ -void stabilizerSetEmergencyStopTimeout(int timeout); - - #endif /* STABILIZER_H_ */ diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index ad9b30fc9d..f101b14c36 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -28,7 +28,15 @@ #include "stabilizer_types.h" -void supervisorUpdate(const sensorData_t *data); +/** + * @brief Update the supervisor. The supervisor will evaluate the current situation to determine if some action is + * required. + * + * @param sensors Sensor values + * @return true Motors are allowed to run + * @return false Motors must not run + */ +bool supervisorUpdate(const sensorData_t *sensors); bool supervisorCanFly(void); bool supervisorIsFlying(void); diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index 54461c19e1..e26dc08e8e 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -59,7 +59,6 @@ #define NBR_OF_SENSOR_DIFFS_IN_PACKET 3 #define NBR_OF_BASESTATIONS 2 #define NBR_OF_ -#define DEFAULT_EMERGENCY_STOP_TIMEOUT (1 * RATE_MAIN_LOOP) typedef enum { @@ -130,6 +129,9 @@ static void extPositionHandler(CRTPPacket* pk); static void genericLocHandle(CRTPPacket* pk); static void extPositionPackedHandler(CRTPPacket* pk); +static bool isEmergencyStopRequested = false; +static uint32_t emergencyStopWatchdogNotificationTick = 0; + void locSrvInit() { if (isInit) { @@ -294,10 +296,10 @@ static void genericLocHandle(CRTPPacket* pk) lpsShortLppPacketHandler(pk); break; case EMERGENCY_STOP: - stabilizerSetEmergencyStop(); + isEmergencyStopRequested = true; break; case EMERGENCY_STOP_WATCHDOG: - stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT); + emergencyStopWatchdogNotificationTick = xTaskGetTickCount(); break; case EXT_POSE: extPoseHandler(pk); @@ -390,6 +392,18 @@ void locSrvSendLighthouseAngle(int baseStation, pulseProcessorResult_t* angles) } #endif +bool locSrvIsEmergencyStopRequested() { + return isEmergencyStopRequested; +} + +void locSrvResetEmergencyStopRequest() { + isEmergencyStopRequested = false; +} + +uint32_t locSrvGetEmergencyStopWatchdogNotificationTick() { + return emergencyStopWatchdogNotificationTick; +} + // This logging group is deprecated (removed after August 2023) LOG_GROUP_START(ext_pos) LOG_ADD(LOG_FLOAT, X, &ext_pos.x) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index c41aa4673e..05a9f361c4 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -58,8 +58,6 @@ #include "rateSupervisor.h" static bool isInit; -static bool emergencyStop = false; -static int emergencyStopTimeout = EMERGENCY_STOP_TIMEOUT_DISABLED; static uint32_t inToOutLatency; @@ -204,17 +202,6 @@ bool stabilizerTest(void) return pass; } -static void checkEmergencyStopTimeout() -{ - if (emergencyStopTimeout >= 0) { - emergencyStopTimeout -= 1; - - if (emergencyStopTimeout == 0) { - emergencyStop = true; - } - } -} - static void batteryCompensation(const motors_thrust_uncapped_t* motorThrustUncapped, motors_thrust_uncapped_t* motorThrustBatCompUncapped) { float supplyVoltage = pmGetBatteryVoltage(); @@ -295,21 +282,19 @@ static void stabilizerTask(void* param) controller(&control, &setpoint, &sensorData, &state, stabilizerStep); - checkEmergencyStopTimeout(); - // // The supervisor module keeps track of Crazyflie state such as if // we are ok to fly, or if the Crazyflie is in flight. // - supervisorUpdate(&sensorData); + const bool areMotorsAllowedToSpin = supervisorUpdate(&sensorData); - if (emergencyStop || (systemIsArmed() == false)) { - motorsStop(); - } else { + if (areMotorsAllowedToSpin) { powerDistribution(&control, &motorThrustUncapped); batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped); powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm); setMotorRatios(&motorPwm); + } else { + motorsStop(); } #ifdef CONFIG_DECK_USD @@ -337,25 +322,9 @@ static void stabilizerTask(void* param) } } -void stabilizerSetEmergencyStop() -{ - emergencyStop = true; -} - -void stabilizerResetEmergencyStop() -{ - emergencyStop = false; -} - -void stabilizerSetEmergencyStopTimeout(int timeout) -{ - emergencyStop = false; - emergencyStopTimeout = timeout; -} - /** * Parameters to set the estimator and controller type - * for the stabilizer module, or to do an emergency stop + * for the stabilizer module */ PARAM_GROUP_START(stabilizer) /** @@ -368,10 +337,6 @@ PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType) * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0) */ PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType) -/** - * @brief If set to nonzero will turn off power - */ -PARAM_ADD_CORE(PARAM_UINT8, stop, &emergencyStop) PARAM_GROUP_STOP(stabilizer) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index f9d6065656..b9c4f0726f 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -29,12 +29,19 @@ #include #include "log.h" +#include "param.h" #include "motors.h" #include "power_distribution.h" #include "pm.h" #include "stabilizer.h" #include "supervisor.h" #include "platform_defaults.h" +#include "crtp_localization_service.h" +#include "system.h" + + +#define DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT (M2T(1000)) + /* Minimum summed motor PWM that means we are flying */ #define SUPERVISOR_FLIGHT_THRESHOLD 1000 @@ -45,6 +52,10 @@ static bool canFly; static bool isFlying; static bool isTumbled; +static bool areMotorsLocked = false; + +// TODO krri +// * Add reset functionality bool supervisorCanFly() { @@ -61,18 +72,6 @@ bool supervisorIsTumbled() return isTumbled; } -// -// We cannot fly if the Crazyflie is tumbled and we cannot fly if the Crazyflie -// is connected to a charger. -// -static bool canFlyCheck() -{ - if (isTumbled) { - return false; - } - return !pmIsChargerConnected(); -} - // // We say we are flying if the sum of the ratios of all motors giving thrust // is above a certain threshold. @@ -114,17 +113,62 @@ static bool isTumbledCheck(const sensorData_t *data) return false; } -void supervisorUpdate(const sensorData_t *data) +static bool checkEmergencyStopWatchdog() { + bool isOk = true; + const uint32_t latestNotification = locSrvGetEmergencyStopWatchdogNotificationTick(); + if (latestNotification > 0) { + const uint32_t tick = xTaskGetTickCount(); + isOk = tick < (latestNotification + DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT); + } + + return isOk; +} + +bool supervisorUpdate(const sensorData_t *sensors) { isFlying = isFlyingCheck(); + isTumbled = isTumbledCheck(sensors); + + bool areMotorsAllowedToSpin = true; - isTumbled = isTumbledCheck(data); - #if SUPERVISOR_TUMBLE_CHECK_ENABLE - if (isTumbled && isFlying) { - stabilizerSetEmergencyStop(); + // canFly is kept for backwards compatibility. TODO krri deprecate? + canFly = true; + + if (isTumbled) { + #if SUPERVISOR_TUMBLE_CHECK_ENABLE + // It is OK to tumble before flying + if (isFlying) { + areMotorsAllowedToSpin = false; + areMotorsLocked = true; + } + #endif + + canFly = false; } - #endif - canFly = canFlyCheck(); + + if (locSrvIsEmergencyStopRequested()) { + areMotorsAllowedToSpin = false; + areMotorsLocked = true; + } + + if (pmIsChargerConnected()) { + areMotorsAllowedToSpin = false; + canFly = false; + } + + if (! checkEmergencyStopWatchdog()) { + areMotorsAllowedToSpin = false; + } + + if (! systemIsArmed()) { + areMotorsAllowedToSpin = false; + } + + if (areMotorsLocked) { + areMotorsAllowedToSpin = false; + } + + return areMotorsAllowedToSpin; } /** @@ -144,3 +188,12 @@ LOG_ADD_CORE(LOG_UINT8, isFlying, &isFlying) */ LOG_ADD_CORE(LOG_UINT8, isTumbled, &isTumbled) LOG_GROUP_STOP(sys) + + +// TODO Krri rename? deprecate? +PARAM_GROUP_START(stabilizer) +/** + * @brief If set to nonzero will turn off power + */ +PARAM_ADD_CORE(PARAM_UINT8, stop, &areMotorsLocked) +PARAM_GROUP_STOP(stabilizer) From d9fc6f99842c35b19ab139d13ae5eadb12d962bd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 19 Apr 2023 15:54:28 +0200 Subject: [PATCH 02/30] Moved setpoint timeout from commander to supervisor --- src/modules/interface/commander.h | 3 - src/modules/interface/supervisor.h | 12 +- src/modules/src/commander.c | 18 +-- src/modules/src/stabilizer.c | 55 +++++---- src/modules/src/supervisor.c | 190 +++++++++++++++++++++-------- 5 files changed, 169 insertions(+), 109 deletions(-) diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h index c852631a67..3798bd52e3 100644 --- a/src/modules/interface/commander.h +++ b/src/modules/interface/commander.h @@ -32,9 +32,6 @@ #define DEFAULT_YAW_MODE XMODE -#define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) -#define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) - #define COMMANDER_PRIORITY_DISABLE 0 // Keep a macro for lowest non-disabled priority, regardless of source, in case // some day there is a priority lower than the high-level commander. diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index f101b14c36..4b90971a85 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -28,15 +28,9 @@ #include "stabilizer_types.h" -/** - * @brief Update the supervisor. The supervisor will evaluate the current situation to determine if some action is - * required. - * - * @param sensors Sensor values - * @return true Motors are allowed to run - * @return false Motors must not run - */ -bool supervisorUpdate(const sensorData_t *sensors); +void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint); + +void supervisorOverrideSetpoint(setpoint_t* setpoint); bool supervisorCanFly(void); bool supervisorIsFlying(void); diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index 45eea716f4..5bd85035c3 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -102,23 +102,7 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state) { xQueuePeek(setpointQueue, setpoint, 0); lastUpdate = setpoint->timestamp; - uint32_t currentTime = xTaskGetTickCount(); - - if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { - memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); - } else if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_STABILIZE) { - xQueueOverwrite(priorityQueue, &priorityDisable); - // Leveling ... - setpoint->mode.x = modeDisable; - setpoint->mode.y = modeDisable; - setpoint->mode.roll = modeAbs; - setpoint->mode.pitch = modeAbs; - setpoint->mode.yaw = modeVelocity; - setpoint->attitude.roll = 0; - setpoint->attitude.pitch = 0; - setpoint->attitudeRate.yaw = 0; - // Keep Z as it is - } + // This copying is not strictly necessary because stabilizer.c already keeps // a static state_t containing the most recent state estimate. However, it is // not accessible by the public interface. diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 05a9f361c4..8e01bae687 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -220,6 +220,25 @@ static void setMotorRatios(const motors_thrust_pwm_t* motorPwm) motorsSetRatio(MOTOR_M4, motorPwm->motors.m4); } +static void updateStateEstimatorAndControllerTypes() { + if (stateEstimatorGetType() != estimatorType) { + stateEstimatorSwitchTo(estimatorType); + estimatorType = stateEstimatorGetType(); + } + + if (controllerGetType() != controllerType) { + controllerInit(controllerType); + controllerType = controllerGetType(); + } +} + +static void controlMotors(const control_t* control) { + powerDistribution(control, &motorThrustUncapped); + batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped); + powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm); + setMotorRatios(&motorPwm); +} + /* The stabilizer loop runs at 1kHz. It is the * responsibility of the different functions to run slower by skipping call * (ie. returning without modifying the output structure). @@ -257,45 +276,27 @@ static void stabilizerTask(void* param) if (healthShallWeRunTest()) { healthRunTests(&sensorData); } else { - // allow to update estimator dynamically - if (stateEstimatorGetType() != estimatorType) { - stateEstimatorSwitchTo(estimatorType); - estimatorType = stateEstimatorGetType(); - } - // allow to update controller dynamically - if (controllerGetType() != controllerType) { - controllerInit(controllerType); - controllerType = controllerGetType(); - } + updateStateEstimatorAndControllerTypes(); stateEstimator(&state, stabilizerStep); - compressState(); if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, stabilizerStep)) { commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL); } - commanderGetSetpoint(&setpoint, &state); - compressSetpoint(); + supervisorUpdate(&sensorData, &setpoint); + // Let the collision avoidance module modify the setpoint, if needed collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep); + supervisorOverrideSetpoint(&setpoint); controller(&control, &setpoint, &sensorData, &state, stabilizerStep); + controlMotors(&control); + // Todo krri: Do we trust the controller to stop the motors if we set the thrust to 0 in the setpoint? Use stopMotors() to be extra sure?? - // - // The supervisor module keeps track of Crazyflie state such as if - // we are ok to fly, or if the Crazyflie is in flight. - // - const bool areMotorsAllowedToSpin = supervisorUpdate(&sensorData); - - if (areMotorsAllowedToSpin) { - powerDistribution(&control, &motorThrustUncapped); - batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped); - powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm); - setMotorRatios(&motorPwm); - } else { - motorsStop(); - } + // Compute compressed log formats + compressState(); + compressSetpoint(); #ifdef CONFIG_DECK_USD // Log data to uSD card if configured diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index b9c4f0726f..3e1ed977c4 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -33,7 +33,6 @@ #include "motors.h" #include "power_distribution.h" #include "pm.h" -#include "stabilizer.h" #include "supervisor.h" #include "platform_defaults.h" #include "crtp_localization_service.h" @@ -42,42 +41,58 @@ #define DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT (M2T(1000)) - /* Minimum summed motor PWM that means we are flying */ #define SUPERVISOR_FLIGHT_THRESHOLD 1000 /* Number of times in a row we need to see a condition before acting upon it */ -#define SUPERVISOR_HYSTERESIS_THRESHOLD 30 +#define TUMBLE_HYSTERESIS_THRESHOLD 30 + +// TODO krri rename +#define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) +#define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) + +typedef enum { + actionNone = 0, // All is normal, we can fly + actionMotorsDisabled, // Motors are disabled for now, for instance when not armed + actionLevelOut, // Level out, all is not lost yet + actionStopMotorsAndFreeFall, // Stop motors permanently and free fall +} action_t; + +typedef struct { + bool canFly; + bool isFlying; + bool isTumbled; + + uint32_t tumbleHysteresis; + + action_t action; +} SupervisorState_t; + +static SupervisorState_t supervisorState; + +const static setpoint_t nullSetpoint; -static bool canFly; -static bool isFlying; -static bool isTumbled; -static bool areMotorsLocked = false; // TODO krri // * Add reset functionality -bool supervisorCanFly() -{ - return canFly; +bool supervisorCanFly() { + return supervisorState.canFly; } -bool supervisorIsFlying() -{ - return isFlying; +bool supervisorIsFlying() { + return supervisorState.isFlying; } -bool supervisorIsTumbled() -{ - return isTumbled; +bool supervisorIsTumbled() { + return supervisorState.isTumbled; } // // We say we are flying if the sum of the ratios of all motors giving thrust // is above a certain threshold. // -static bool isFlyingCheck() -{ +static bool isFlyingCheck() { int sumRatio = 0; for (int i = 0; i < NBR_OF_MOTORS; ++i) { sumRatio += powerDistributionMotorType(i) * motorsGetRatio(i); @@ -93,84 +108,151 @@ static bool isFlyingCheck() // the thrust to the motors, avoiding the Crazyflie from running propellers at // significant thrust when accidentally crashing into walls or the ground. // -static bool isTumbledCheck(const sensorData_t *data) -{ +static bool isTumbledCheck(SupervisorState_t* this, const sensorData_t *data) { const float tolerance = -0.5; - static uint32_t hysteresis = 0; // - // We need a SUPERVISOR_HYSTERESIS_THRESHOLD amount of readings that indicate + // We need a TUMBLE_HYSTERESIS_THRESHOLD amount of readings that indicate // that we are tumbled before we act on it. This is to reduce false positives. // if (data->acc.z <= tolerance) { - hysteresis++; - if (hysteresis > SUPERVISOR_HYSTERESIS_THRESHOLD) { + this->tumbleHysteresis++; + if (this->tumbleHysteresis > TUMBLE_HYSTERESIS_THRESHOLD) { return true; } } else { - hysteresis = 0; + this->tumbleHysteresis = 0; } return false; } -static bool checkEmergencyStopWatchdog() { +static bool checkEmergencyStopWatchdog(const uint32_t tick) { bool isOk = true; + const uint32_t latestNotification = locSrvGetEmergencyStopWatchdogNotificationTick(); if (latestNotification > 0) { - const uint32_t tick = xTaskGetTickCount(); isOk = tick < (latestNotification + DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT); } return isOk; } -bool supervisorUpdate(const sensorData_t *sensors) -{ - isFlying = isFlyingCheck(); - isTumbled = isTumbledCheck(sensors); - bool areMotorsAllowedToSpin = true; +static action_t setAction(SupervisorState_t* this, const action_t newAction) { + switch(this->action) { + case actionNone: + this->action = newAction; + break; + case actionMotorsDisabled: + if (newAction == actionNone || newAction == actionStopMotorsAndFreeFall) { + this->action = newAction; + } + break; + case actionLevelOut: + if (newAction == actionNone || newAction == actionStopMotorsAndFreeFall) { + this->action = newAction; + } + break; + case actionStopMotorsAndFreeFall: + // Intentionally empty + break; + default: + ASSERT_FAILED(); + break; + } + + return this->action; +} + +void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { + SupervisorState_t* this = &supervisorState; + const uint32_t currentTick = xTaskGetTickCount(); + + this->isFlying = isFlyingCheck(); + this->isTumbled = isTumbledCheck(this, sensors); // canFly is kept for backwards compatibility. TODO krri deprecate? - canFly = true; + this->canFly = true; + + // Reset action (if possible) + setAction(this, actionNone); - if (isTumbled) { + if (this->isTumbled) { #if SUPERVISOR_TUMBLE_CHECK_ENABLE - // It is OK to tumble before flying - if (isFlying) { - areMotorsAllowedToSpin = false; - areMotorsLocked = true; + if (this->isFlying) { + setAction(this, actionStopMotorsAndFreeFall); + } else { + setAction(this, actionMotorsDisabled); } #endif - canFly = false; + this->canFly = false; } if (locSrvIsEmergencyStopRequested()) { - areMotorsAllowedToSpin = false; - areMotorsLocked = true; + setAction(this, actionStopMotorsAndFreeFall); } if (pmIsChargerConnected()) { - areMotorsAllowedToSpin = false; - canFly = false; + setAction(this, actionMotorsDisabled); + this->canFly = false; } - if (! checkEmergencyStopWatchdog()) { - areMotorsAllowedToSpin = false; + if (! checkEmergencyStopWatchdog(currentTick)) { + if (this->isFlying){ + setAction(this, actionStopMotorsAndFreeFall); + } else { + setAction(this, actionMotorsDisabled); + } } if (! systemIsArmed()) { - areMotorsAllowedToSpin = false; + setAction(this, actionMotorsDisabled); } - if (areMotorsLocked) { - areMotorsAllowedToSpin = false; + const uint32_t setpointAge = currentTick - setpoint->timestamp; + if (this->isFlying) { + if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { + setAction(this, actionLevelOut); + } + + if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { + setAction(this, actionStopMotorsAndFreeFall); + } + } else { + if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { + setAction(this, actionMotorsDisabled); + } } +} + - return areMotorsAllowedToSpin; +void supervisorOverrideSetpoint(setpoint_t* setpoint) { + SupervisorState_t* this = &supervisorState; + switch(this->action){ + case actionNone: + // Do nothing + break; + case actionLevelOut: + setpoint->mode.x = modeDisable; + setpoint->mode.y = modeDisable; + setpoint->mode.roll = modeAbs; + setpoint->mode.pitch = modeAbs; + setpoint->mode.yaw = modeVelocity; + setpoint->attitude.roll = 0; + setpoint->attitude.pitch = 0; + setpoint->attitudeRate.yaw = 0; + // Keep Z as it is + break; + case actionMotorsDisabled: + // Fall through + case actionStopMotorsAndFreeFall: + memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); + break; + } } + /** * System loggable variables to check different system states. */ @@ -178,15 +260,15 @@ LOG_GROUP_START(sys) /** * @brief If nonzero if system is ready to fly. */ -LOG_ADD_CORE(LOG_UINT8, canfly, &canFly) +LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorState.canFly) /** * @brief Nonzero if the system thinks it is flying */ -LOG_ADD_CORE(LOG_UINT8, isFlying, &isFlying) +LOG_ADD_CORE(LOG_UINT8, isFlying, &supervisorState.isFlying) /** * @brief Nonzero if the system thinks it is tumbled/crashed */ -LOG_ADD_CORE(LOG_UINT8, isTumbled, &isTumbled) +LOG_ADD_CORE(LOG_UINT8, isTumbled, &supervisorState.isTumbled) LOG_GROUP_STOP(sys) @@ -195,5 +277,7 @@ PARAM_GROUP_START(stabilizer) /** * @brief If set to nonzero will turn off power */ -PARAM_ADD_CORE(PARAM_UINT8, stop, &areMotorsLocked) + +// TODO krri How to handle? +// PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorState.areMotorsLocked) PARAM_GROUP_STOP(stabilizer) From 50636ebff1809d60055260bff9bf066c1811633d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 19 Apr 2023 16:16:52 +0200 Subject: [PATCH 03/30] Add default handling --- src/modules/src/supervisor.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 3e1ed977c4..ccfb0a74d2 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -247,6 +247,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { case actionMotorsDisabled: // Fall through case actionStopMotorsAndFreeFall: + // Fall through + default: memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); break; } From e374aa73f65d76d96ffb40b4546d9042c4060fb2 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Apr 2023 12:28:30 +0200 Subject: [PATCH 04/30] Added low level motor stop --- src/modules/interface/supervisor.h | 1 + src/modules/src/stabilizer.c | 16 ++++++++++++++-- src/modules/src/supervisor.c | 6 +++++- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index 4b90971a85..9742a061f4 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -31,6 +31,7 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint); void supervisorOverrideSetpoint(setpoint_t* setpoint); +bool supervisorAreMotorsAllowedToRun(); bool supervisorCanFly(void); bool supervisorIsFlying(void); diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 8e01bae687..d24cafde07 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -284,15 +284,27 @@ static void stabilizerTask(void* param) commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL); } commanderGetSetpoint(&setpoint, &state); + + // Critical for safety, be careful if you modify this code! + // Let the supervisor update it's view of the current situation supervisorUpdate(&sensorData, &setpoint); // Let the collision avoidance module modify the setpoint, if needed collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep); + + // Critical for safety, be careful if you modify this code! + // Let the supervisor modify the setpoint to handle exceptional conditions supervisorOverrideSetpoint(&setpoint); controller(&control, &setpoint, &sensorData, &state, stabilizerStep); - controlMotors(&control); - // Todo krri: Do we trust the controller to stop the motors if we set the thrust to 0 in the setpoint? Use stopMotors() to be extra sure?? + + // Critical for safety, be careful if you modify this code! + // The supervisor will already set thrust to 0 in the setpoint if needed, but to be extra sure prevent motors from running. + if (supervisorAreMotorsAllowedToRun()) { + controlMotors(&control); + } else { + motorsStop(); + } // Compute compressed log formats compressState(); diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index ccfb0a74d2..40daee812c 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -157,7 +157,7 @@ static action_t setAction(SupervisorState_t* this, const action_t newAction) { // Intentionally empty break; default: - ASSERT_FAILED(); + // Do nothing break; } @@ -254,6 +254,10 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { } } +bool supervisorAreMotorsAllowedToRun() { + SupervisorState_t* this = &supervisorState; + return (this->action == actionNone) || (this->action == actionLevelOut); +} /** * System loggable variables to check different system states. From 63bee47afb9ed4d69bb920119c1bfea32219dda7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Apr 2023 15:52:04 +0200 Subject: [PATCH 05/30] Added supervisor state diagram --- docs/functional-areas/supervisor/index.md | 9 ++++ docs/functional-areas/supervisor/states.md | 53 ++++++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 docs/functional-areas/supervisor/index.md create mode 100644 docs/functional-areas/supervisor/states.md diff --git a/docs/functional-areas/supervisor/index.md b/docs/functional-areas/supervisor/index.md new file mode 100644 index 0000000000..be17d57cf5 --- /dev/null +++ b/docs/functional-areas/supervisor/index.md @@ -0,0 +1,9 @@ +--- +title: The supervisor +page_id: supervisor_index +--- + +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. + +{% sub_page_menu %} diff --git a/docs/functional-areas/supervisor/states.md b/docs/functional-areas/supervisor/states.md new file mode 100644 index 0000000000..a87aeed977 --- /dev/null +++ b/docs/functional-areas/supervisor/states.md @@ -0,0 +1,53 @@ +--- +title: Supervisor states +page_id: supervisor_states +--- +{% ditaa --alt "Supervisor states" %} + Boot| + V ++-------------------+ ++ Pre-flight checks +<------------+ ++ not passed +<--+ | ++-------+-----------+ | | + | ^ | | + V | | | ++---------+---------+ | | ++ Pre-flight checks + | | ++ passed + | | ++-------+-----------+ | | + | ^ | +-------+---------+ + Arming| | | + Reset + + V | | + + ++---------+---------+ | +-----------------+ ++ Ready to fly +---+ ^ ^ ++ + | | ++--------+----------+ | | + | | | + V | | ++-------------------+ | | +-------------------+ ++ Flying +------------|-|------>+ Exception + ++ +------------|-|---+ + Free fall +--+ ++--------+----------+ | | | +-------------------+ | + | | | | | + V | | | | ++-------------------+ | | | +-------------------+ | ++ Landed +------------+ | +-->+ Exception + | ++ | | + Soft fall + | ++--------+----------+ | +---------+---------+ | + | | | | + | | | +---------+ + | | | | + | | V V + +---------------+ | +-------------------+ + | +-------+ Platform not + + | + moving + + | +---------+---------+ + | | + | +--------------+ + | | + V V + +-------------------+ + + Lock + + + + + +-------------------+ +{% endditaa %} From 5a9be21372a8ea8fdd762167887c611d90a353ec Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 20 Apr 2023 16:58:25 +0200 Subject: [PATCH 06/30] Added supervisor state machine --- .../interface/supervisor_state_machine.h | 82 ++++++ src/modules/src/Kbuild | 1 + src/modules/src/stabilizer.c | 4 +- src/modules/src/supervisor.c | 151 +++++----- src/modules/src/supervisor_state_machine.c | 206 ++++++++++++++ .../src/test_supervisor_state_machine.c | 263 ++++++++++++++++++ 6 files changed, 619 insertions(+), 88 deletions(-) create mode 100644 src/modules/interface/supervisor_state_machine.h create mode 100644 src/modules/src/supervisor_state_machine.c create mode 100644 test/modules/src/test_supervisor_state_machine.c diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h new file mode 100644 index 0000000000..51116c1a0a --- /dev/null +++ b/src/modules/interface/supervisor_state_machine.h @@ -0,0 +1,82 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2023 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +typedef enum { + supervisorStatePreFlChecksNotPassed = 0, + supervisorStatePreFlChecksPassed, + supervisorStateReadyToFly, + supervisorStateFlying, + supervisorStateLanded, + supervisorStateReset, + supervisorStateWarningLevelOut, + supervisorStateExceptFreeFall, + supervisorStateExceptNotMoving, + supervisorState_NrOfStates, +} supervisorState_t; + +// Conditions supported by the supervisor +enum { + supervisorConditionArmed = 0, + supervisorConditionChargerConnected, + supervisorConditionIsFlying, + supervisorConditionIsTumbled, + supervisorConditionIsMoving, + supervisorConditionCommanderWdtWarning, + supervisorConditionCommanderWdtTimeout, + supervisorConditionEmergencyStop, +}; + +typedef uint32_t supervisorConditionBit_t; + +// Condition bit definitions +#define SUPERVISOR_TB_NONE (0) +#define SUPERVISOR_TB_ARMED (1 << supervisorConditionArmed) +#define SUPERVISOR_TB_CHARGER_CONNECTED (1 << supervisorConditionChargerConnected) +#define SUPERVISOR_TB_IS_FLYING (1 << supervisorConditionIsFlying) +#define SUPERVISOR_TB_IS_TUMBLED (1 << supervisorConditionIsTumbled) +#define SUPERVISOR_TB_IS_MOVING (1 << supervisorConditionIsMoving) +#define SUPERVISOR_TB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning) +#define SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout) +#define SUPERVISOR_TB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop) + + +typedef struct { + supervisorState_t newState; + supervisorConditionBit_t mustBeSet; + supervisorConditionBit_t mustNotBeSet; +} SupervisorStateTransition_t; + +typedef struct { + SupervisorStateTransition_t* transitionList; + int length; +} SupervisorStateTransitionList_t; + +// Macro used when defining SupervisorStateTransitionLists +#define SUPERVISOR_TRANSITION_ENTRY(TRANSITION_DEF) .transitionList=TRANSITION_DEF, .length=(sizeof(TRANSITION_DEF) / sizeof(SupervisorStateTransition_t)) + +supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBit_t conditions); diff --git a/src/modules/src/Kbuild b/src/modules/src/Kbuild index a845343637..0a998a9a43 100644 --- a/src/modules/src/Kbuild +++ b/src/modules/src/Kbuild @@ -41,6 +41,7 @@ obj-y += sound_cf2.o obj-y += stabilizer.o obj-y += static_mem.o obj-y += supervisor.o +obj-y += supervisor_state_machine.o obj-y += sysload.o obj-y += system.o obj-$(CONFIG_DECK_LOCO) += tdoaEngineInstance.o diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index d24cafde07..2e146fa912 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -262,10 +262,10 @@ static void stabilizerTask(void* param) // Initialize stabilizerStep to something else than 0 stabilizerStep = 1; + systemWaitStart(); + DEBUG_PRINT("Starting stabilizer loop\n"); rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1); - DEBUG_PRINT("Ready to fly.\n"); - while(1) { // The sensor should unlock at 1kHz sensorsWaitDataReady(); diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 40daee812c..61bef08072 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -7,7 +7,7 @@ * * Crazyflie control firmware * -* Copyright (C) 2021 Bitcraze AB +* Copyright (C) 2021 - 2023 Bitcraze AB * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -34,6 +34,7 @@ #include "power_distribution.h" #include "pm.h" #include "supervisor.h" +#include "supervisor_state_machine.h" #include "platform_defaults.h" #include "crtp_localization_service.h" #include "system.h" @@ -66,9 +67,10 @@ typedef struct { uint32_t tumbleHysteresis; action_t action; -} SupervisorState_t; + supervisorState_t state; +} SupervisorMem_t; -static SupervisorState_t supervisorState; +static SupervisorMem_t supervisorMem; const static setpoint_t nullSetpoint; @@ -77,15 +79,15 @@ const static setpoint_t nullSetpoint; // * Add reset functionality bool supervisorCanFly() { - return supervisorState.canFly; + return supervisorMem.canFly; } bool supervisorIsFlying() { - return supervisorState.isFlying; + return supervisorMem.isFlying; } bool supervisorIsTumbled() { - return supervisorState.isTumbled; + return supervisorMem.isTumbled; } // @@ -108,7 +110,7 @@ static bool isFlyingCheck() { // the thrust to the motors, avoiding the Crazyflie from running propellers at // significant thrust when accidentally crashing into walls or the ground. // -static bool isTumbledCheck(SupervisorState_t* this, const sensorData_t *data) { +static bool isTumbledCheck(SupervisorMem_t* this, const sensorData_t *data) { const float tolerance = -0.5; // // We need a TUMBLE_HYSTERESIS_THRESHOLD amount of readings that indicate @@ -126,6 +128,11 @@ static bool isTumbledCheck(SupervisorState_t* this, const sensorData_t *data) { return false; } +static bool isMovingCheck(SupervisorMem_t* this, const sensorData_t *data) { + // TODO krri implement + return true; +} + static bool checkEmergencyStopWatchdog(const uint32_t tick) { bool isOk = true; @@ -138,34 +145,8 @@ static bool checkEmergencyStopWatchdog(const uint32_t tick) { } -static action_t setAction(SupervisorState_t* this, const action_t newAction) { - switch(this->action) { - case actionNone: - this->action = newAction; - break; - case actionMotorsDisabled: - if (newAction == actionNone || newAction == actionStopMotorsAndFreeFall) { - this->action = newAction; - } - break; - case actionLevelOut: - if (newAction == actionNone || newAction == actionStopMotorsAndFreeFall) { - this->action = newAction; - } - break; - case actionStopMotorsAndFreeFall: - // Intentionally empty - break; - default: - // Do nothing - break; - } - - return this->action; -} - void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { - SupervisorState_t* this = &supervisorState; + SupervisorMem_t* this = &supervisorMem; const uint32_t currentTick = xTaskGetTickCount(); this->isFlying = isFlyingCheck(); @@ -174,66 +155,55 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { // canFly is kept for backwards compatibility. TODO krri deprecate? this->canFly = true; - // Reset action (if possible) - setAction(this, actionNone); + supervisorConditionBit_t conditions = 0; + if (systemIsArmed()) { + conditions |= SUPERVISOR_TB_ARMED; + } + if (pmIsChargerConnected()) { + conditions |= SUPERVISOR_TB_CHARGER_CONNECTED; + } + if (this->isFlying) { + conditions |= SUPERVISOR_TB_IS_FLYING; + } if (this->isTumbled) { - #if SUPERVISOR_TUMBLE_CHECK_ENABLE - if (this->isFlying) { - setAction(this, actionStopMotorsAndFreeFall); - } else { - setAction(this, actionMotorsDisabled); - } - #endif - - this->canFly = false; + conditions |= SUPERVISOR_TB_IS_TUMBLED; } - - if (locSrvIsEmergencyStopRequested()) { - setAction(this, actionStopMotorsAndFreeFall); + if (isMovingCheck(this, sensors)) { + conditions |= SUPERVISOR_TB_IS_MOVING; } - - if (pmIsChargerConnected()) { - setAction(this, actionMotorsDisabled); - this->canFly = false; + const uint32_t setpointAge = currentTick - setpoint->timestamp; + if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { + conditions |= SUPERVISOR_TB_COMMANDER_WDT_WARNING; } - - if (! checkEmergencyStopWatchdog(currentTick)) { - if (this->isFlying){ - setAction(this, actionStopMotorsAndFreeFall); - } else { - setAction(this, actionMotorsDisabled); - } + if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { + conditions |= SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT; } - - if (! systemIsArmed()) { - setAction(this, actionMotorsDisabled); + if (!checkEmergencyStopWatchdog(currentTick)) { + conditions |= SUPERVISOR_TB_EMERGENCY_STOP; + } + if (locSrvIsEmergencyStopRequested()) { + conditions |= SUPERVISOR_TB_EMERGENCY_STOP; } - const uint32_t setpointAge = currentTick - setpoint->timestamp; - if (this->isFlying) { - if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { - setAction(this, actionLevelOut); - } + supervisorState_t newState = supervisorStateUpdate(this->state, conditions); - if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { - setAction(this, actionStopMotorsAndFreeFall); - } - } else { - if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { - setAction(this, actionMotorsDisabled); - } - } + // TODO krri add transition actions? + + this->state = newState; } void supervisorOverrideSetpoint(setpoint_t* setpoint) { - SupervisorState_t* this = &supervisorState; - switch(this->action){ - case actionNone: + SupervisorMem_t* this = &supervisorMem; + switch(this->state){ + case supervisorStateReadyToFly: + // Fall through + case supervisorStateFlying: // Do nothing break; - case actionLevelOut: + + case supervisorStateWarningLevelOut: setpoint->mode.x = modeDisable; setpoint->mode.y = modeDisable; setpoint->mode.roll = modeAbs; @@ -244,9 +214,18 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { setpoint->attitudeRate.yaw = 0; // Keep Z as it is break; - case actionMotorsDisabled: + + case supervisorStateLanded: + // Fall through + case supervisorStateReset: + // Fall through + case supervisorStateExceptFreeFall: + // Fall through + case supervisorStateExceptNotMoving: + // Fall through + case supervisorStatePreFlChecksNotPassed: // Fall through - case actionStopMotorsAndFreeFall: + case supervisorStatePreFlChecksPassed: // Fall through default: memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); @@ -255,7 +234,7 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { } bool supervisorAreMotorsAllowedToRun() { - SupervisorState_t* this = &supervisorState; + SupervisorMem_t* this = &supervisorMem; return (this->action == actionNone) || (this->action == actionLevelOut); } @@ -266,15 +245,15 @@ LOG_GROUP_START(sys) /** * @brief If nonzero if system is ready to fly. */ -LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorState.canFly) +LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorMem.canFly) /** * @brief Nonzero if the system thinks it is flying */ -LOG_ADD_CORE(LOG_UINT8, isFlying, &supervisorState.isFlying) +LOG_ADD_CORE(LOG_UINT8, isFlying, &supervisorMem.isFlying) /** * @brief Nonzero if the system thinks it is tumbled/crashed */ -LOG_ADD_CORE(LOG_UINT8, isTumbled, &supervisorState.isTumbled) +LOG_ADD_CORE(LOG_UINT8, isTumbled, &supervisorMem.isTumbled) LOG_GROUP_STOP(sys) @@ -285,5 +264,5 @@ PARAM_GROUP_START(stabilizer) */ // TODO krri How to handle? -// PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorState.areMotorsLocked) +// PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.areMotorsLocked) PARAM_GROUP_STOP(stabilizer) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c new file mode 100644 index 0000000000..340f97fa76 --- /dev/null +++ b/src/modules/src/supervisor_state_machine.c @@ -0,0 +1,206 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * Crazyflie control firmware + * + * Copyright (C) 2023 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include "supervisor_state_machine.h" +#include "test_support.h" + +#define DEBUG_ME + +#ifdef DEBUG_ME +#define DEBUG_MODULE "SupSt" +#include "debug.h" +#include "cfassert.h" + +static const char* const stateNames[] = { + "Pre-flight checks not passed", + "Pre-flight checks passed", + "Ready to fly", + "Flying", + "Landed", + "Reset", + "Warning, level out", + "Exception, free fall", + "Exception, not moving", +}; +static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates); +#endif + + +// State transition definitions +static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { + { + .newState = supervisorStatePreFlChecksPassed, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP + } +}; + +static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { + { + .newState = supervisorStateReadyToFly, + .mustBeSet = SUPERVISOR_TB_ARMED, + .mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP + } +}; + +static SupervisorStateTransition_t transitionsReadyToFly[] = { + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_ARMED + }, + { + .newState = supervisorStateFlying, + .mustBeSet = SUPERVISOR_TB_IS_FLYING, + .mustNotBeSet = SUPERVISOR_TB_NONE + } +}; + +static SupervisorStateTransition_t transitionsFlying[] = { + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, + { + .newState = supervisorStateWarningLevelOut, + .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_WARNING, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, + { + .newState = supervisorStateLanded, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_IS_FLYING + } +}; + +static SupervisorStateTransition_t transitionsLanded[] = { + { + .newState = supervisorStateReset, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_IS_MOVING + }, +}; + +static SupervisorStateTransition_t transitionsReset[] = { + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, +}; + +static SupervisorStateTransition_t transitionsWarningLevelOut[] = { + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, + { + .newState = supervisorStateFlying, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_COMMANDER_WDT_WARNING | SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT + }, +}; + +static SupervisorStateTransition_t transitionsExceptFreeFall[] = { + { + .newState = supervisorStateExceptNotMoving, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_IS_MOVING + }, +}; + +static SupervisorStateTransition_t transitionsExceptNotMoving[] = { + { + .newState = supervisorStateReset, + .mustBeSet = SUPERVISOR_TB_NONE, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, +}; + +SupervisorStateTransitionList_t transitionLists[] = { + {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksNotPassed)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsPreFlChecksPassed)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsReadyToFly)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsFlying)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsLanded)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsReset)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsWarningLevelOut)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptFreeFall)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptNotMoving)}, +}; +static_assert(sizeof(transitionLists) / sizeof(transitionLists[0]) == supervisorState_NrOfStates); + + +bool supervisorStateMachineInit() { + if (sizeof(supervisorState_t) != sizeof(transitionLists)) { + return false; + } + + return true; +} + +TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBit_t conditions, const SupervisorStateTransitionList_t* transitions) { + supervisorState_t newState = currentState; + for (int i = 0; i < transitions->length; i++) { + const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; + + const supervisorConditionBit_t maskPos = transitionDef->mustBeSet; + const supervisorConditionBit_t conditionsNotMetPos = (~conditions) & maskPos; + + const supervisorConditionBit_t maskNeg = transitionDef->mustNotBeSet; + const supervisorConditionBit_t conditionsNotMetNeg = conditions & maskNeg; + + const supervisorConditionBit_t conditionsNotMet = conditionsNotMetPos | conditionsNotMetNeg; + + if (conditionsNotMet == 0) { + newState = transitionDef->newState; + break; + } + } + return newState; +} + +supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBit_t conditions) { + #ifdef DEBUG_ME + ASSERT(currentState < supervisorState_NrOfStates); + #endif + + const SupervisorStateTransitionList_t* transitions = &transitionLists[currentState]; + const supervisorState_t newState = findTransition(currentState, conditions, transitions); + + #ifdef DEBUG_ME + ASSERT(currentState < supervisorState_NrOfStates); + ASSERT(newState < supervisorState_NrOfStates); + + if (newState != currentState) { + DEBUG_PRINT("Enter %s -> %s\n", stateNames[currentState], stateNames[newState]); + } + #endif + + return newState; +} diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c new file mode 100644 index 0000000000..1af8497d9f --- /dev/null +++ b/test/modules/src/test_supervisor_state_machine.c @@ -0,0 +1,263 @@ +// File under test supervisor_state_machine.h +#include "supervisor_state_machine.h" + +#include +#include + +#include "unity.h" + +// Function under test +supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBit_t triggerBitField, const SupervisorStateTransitionList_t* transitions); + +// Helpers +static void assertStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet); +static void assertNoStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet); + +void setUp(void) { + // Empty +} + +void tearDown(void) { + // Empty +} + +void testTransitionWithNoConditions(void) { + // Fixture + supervisorConditionBit_t conditions = 123; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionOnePositiveConditionMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionOnePositiveConditionNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveConditionsMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveConditionsMetWithOtherPositives(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveConditionsOneMissing(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveConditionsOneMissingButOtherPositives(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionOneNegativeConditionMet(void) { + // Fixture + supervisorConditionBit_t conditions = 0; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionOneNegativeConditionNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionOneNegativeConditionNotMetWithOtherPositives(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiNegativeConditionsMet(void) { + // Fixture + supervisorConditionBit_t conditions = 0; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiNegativeConditionsOneNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveAndNegativeConditionsMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + + // Test + // Assert + assertStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveAndNegativeConditionsOnePositiveNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveAndNegativeConditionsOneNegativeNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testTransitionMultiPositiveAndNegativeConditionsMultipleNotMet(void) { + // Fixture + supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + + // Test + // Assert + assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); +} + +void testFirstValidTransitionIsChosen(void) { + // Fixture + const supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; + + const supervisorState_t currentState = supervisorStateFlying; + const supervisorState_t expected = supervisorStateExceptFreeFall; + + SupervisorStateTransition_t transitions[] = { + { + .newState = supervisorStateLanded, + .mustBeSet = SUPERVISOR_TB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_TB_EMERGENCY_STOP + }, + { // We expect this state to be chosen + .newState = expected, + .mustBeSet = SUPERVISOR_TB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_TB_NONE + }, + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_TB_NONE + } + }; + + SupervisorStateTransitionList_t transitionsDef = {SUPERVISOR_TRANSITION_ENTRY(transitions)}; + + // Test + const supervisorState_t actual = findTransition(currentState, conditions, &transitionsDef); + + // ASSERT + TEST_ASSERT_EQUAL(expected, actual); +} + +// Helpers //////////////////////////////////////////////// + +static bool check_state_transition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { + // Fixture + const supervisorState_t currentState = supervisorStateFlying; + + SupervisorStateTransition_t transitions[] = { + { + .newState = supervisorStateLanded, + .mustBeSet = mustBeSet, + .mustNotBeSet = mustNotBeSet + } + }; + + SupervisorStateTransitionList_t transitionsDef = {SUPERVISOR_TRANSITION_ENTRY(transitions)}; + + // Test + const supervisorState_t newState = findTransition(currentState, conditions, &transitionsDef); + + // We did get a transition if the new state is different from the current state + return newState != currentState; +} + +static void assertStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { + TEST_ASSERT_TRUE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) +} + +static void assertNoStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { + TEST_ASSERT_FALSE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) +} From 8597cf59fe82895c43418de8cb792374cc5cdcd5 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 13:41:23 +0200 Subject: [PATCH 07/30] Refactoring --- .../interface/supervisor_state_machine.h | 26 ++-- src/modules/src/supervisor.c | 20 +-- src/modules/src/supervisor_state_machine.c | 66 +++++----- .../src/test_supervisor_state_machine.c | 122 +++++++++--------- 4 files changed, 117 insertions(+), 117 deletions(-) diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h index 51116c1a0a..5639e75f5d 100644 --- a/src/modules/interface/supervisor_state_machine.h +++ b/src/modules/interface/supervisor_state_machine.h @@ -51,24 +51,24 @@ enum { supervisorConditionEmergencyStop, }; -typedef uint32_t supervisorConditionBit_t; +typedef uint32_t supervisorConditionBits_t; // Condition bit definitions -#define SUPERVISOR_TB_NONE (0) -#define SUPERVISOR_TB_ARMED (1 << supervisorConditionArmed) -#define SUPERVISOR_TB_CHARGER_CONNECTED (1 << supervisorConditionChargerConnected) -#define SUPERVISOR_TB_IS_FLYING (1 << supervisorConditionIsFlying) -#define SUPERVISOR_TB_IS_TUMBLED (1 << supervisorConditionIsTumbled) -#define SUPERVISOR_TB_IS_MOVING (1 << supervisorConditionIsMoving) -#define SUPERVISOR_TB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning) -#define SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout) -#define SUPERVISOR_TB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop) +#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_IS_MOVING (1 << supervisorConditionIsMoving) +#define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning) +#define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout) +#define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop) typedef struct { supervisorState_t newState; - supervisorConditionBit_t mustBeSet; - supervisorConditionBit_t mustNotBeSet; + supervisorConditionBits_t mustBeSet; + supervisorConditionBits_t mustNotBeSet; } SupervisorStateTransition_t; typedef struct { @@ -79,4 +79,4 @@ typedef struct { // Macro used when defining SupervisorStateTransitionLists #define SUPERVISOR_TRANSITION_ENTRY(TRANSITION_DEF) .transitionList=TRANSITION_DEF, .length=(sizeof(TRANSITION_DEF) / sizeof(SupervisorStateTransition_t)) -supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBit_t conditions); +supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBits_t conditions); diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 61bef08072..1749f9260a 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -156,34 +156,34 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { this->canFly = true; - supervisorConditionBit_t conditions = 0; + supervisorConditionBits_t conditions = 0; if (systemIsArmed()) { - conditions |= SUPERVISOR_TB_ARMED; + conditions |= SUPERVISOR_CB_ARMED; } if (pmIsChargerConnected()) { - conditions |= SUPERVISOR_TB_CHARGER_CONNECTED; + conditions |= SUPERVISOR_CB_CHARGER_CONNECTED; } if (this->isFlying) { - conditions |= SUPERVISOR_TB_IS_FLYING; + conditions |= SUPERVISOR_CB_IS_FLYING; } if (this->isTumbled) { - conditions |= SUPERVISOR_TB_IS_TUMBLED; + conditions |= SUPERVISOR_CB_IS_TUMBLED; } if (isMovingCheck(this, sensors)) { - conditions |= SUPERVISOR_TB_IS_MOVING; + conditions |= SUPERVISOR_CB_IS_MOVING; } const uint32_t setpointAge = currentTick - setpoint->timestamp; if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { - conditions |= SUPERVISOR_TB_COMMANDER_WDT_WARNING; + conditions |= SUPERVISOR_CB_COMMANDER_WDT_WARNING; } if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { - conditions |= SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT; + conditions |= SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT; } if (!checkEmergencyStopWatchdog(currentTick)) { - conditions |= SUPERVISOR_TB_EMERGENCY_STOP; + conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } if (locSrvIsEmergencyStopRequested()) { - conditions |= SUPERVISOR_TB_EMERGENCY_STOP; + conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } supervisorState_t newState = supervisorStateUpdate(this->state, conditions); diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 340f97fa76..df93a1e055 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -53,92 +53,92 @@ static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOf static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { { .newState = supervisorStatePreFlChecksPassed, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP } }; static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { { .newState = supervisorStateReadyToFly, - .mustBeSet = SUPERVISOR_TB_ARMED, - .mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP + .mustBeSet = SUPERVISOR_CB_ARMED, + .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP } }; static SupervisorStateTransition_t transitionsReadyToFly[] = { { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_ARMED + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_ARMED }, { .newState = supervisorStateFlying, - .mustBeSet = SUPERVISOR_TB_IS_FLYING, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_IS_FLYING, + .mustNotBeSet = SUPERVISOR_CB_NONE } }; static SupervisorStateTransition_t transitionsFlying[] = { { .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, + .mustNotBeSet = SUPERVISOR_CB_NONE }, { .newState = supervisorStateWarningLevelOut, - .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_WARNING, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING, + .mustNotBeSet = SUPERVISOR_CB_NONE }, { .newState = supervisorStateLanded, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_IS_FLYING + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_IS_FLYING } }; static SupervisorStateTransition_t transitionsLanded[] = { { .newState = supervisorStateReset, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_IS_MOVING + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_IS_MOVING }, }; static SupervisorStateTransition_t transitionsReset[] = { { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_NONE }, }; static SupervisorStateTransition_t transitionsWarningLevelOut[] = { { .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, + .mustNotBeSet = SUPERVISOR_CB_NONE }, { .newState = supervisorStateFlying, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_COMMANDER_WDT_WARNING | SUPERVISOR_TB_COMMANDER_WDT_TIMEOUT + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING | SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT }, }; static SupervisorStateTransition_t transitionsExceptFreeFall[] = { { .newState = supervisorStateExceptNotMoving, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_IS_MOVING + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_IS_MOVING }, }; static SupervisorStateTransition_t transitionsExceptNotMoving[] = { { .newState = supervisorStateReset, - .mustBeSet = SUPERVISOR_TB_NONE, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_NONE }, }; @@ -164,18 +164,18 @@ bool supervisorStateMachineInit() { return true; } -TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBit_t conditions, const SupervisorStateTransitionList_t* transitions) { +TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBits_t conditions, const SupervisorStateTransitionList_t* transitions) { supervisorState_t newState = currentState; for (int i = 0; i < transitions->length; i++) { const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; - const supervisorConditionBit_t maskPos = transitionDef->mustBeSet; - const supervisorConditionBit_t conditionsNotMetPos = (~conditions) & maskPos; + const supervisorConditionBits_t maskPos = transitionDef->mustBeSet; + const supervisorConditionBits_t conditionsNotMetPos = (~conditions) & maskPos; - const supervisorConditionBit_t maskNeg = transitionDef->mustNotBeSet; - const supervisorConditionBit_t conditionsNotMetNeg = conditions & maskNeg; + const supervisorConditionBits_t maskNeg = transitionDef->mustNotBeSet; + const supervisorConditionBits_t conditionsNotMetNeg = conditions & maskNeg; - const supervisorConditionBit_t conditionsNotMet = conditionsNotMetPos | conditionsNotMetNeg; + const supervisorConditionBits_t conditionsNotMet = conditionsNotMetPos | conditionsNotMetNeg; if (conditionsNotMet == 0) { newState = transitionDef->newState; @@ -185,7 +185,7 @@ TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t current return newState; } -supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBit_t conditions) { +supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBits_t conditions) { #ifdef DEBUG_ME ASSERT(currentState < supervisorState_NrOfStates); #endif diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c index 1af8497d9f..08a20af282 100644 --- a/test/modules/src/test_supervisor_state_machine.c +++ b/test/modules/src/test_supervisor_state_machine.c @@ -7,11 +7,11 @@ #include "unity.h" // Function under test -supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBit_t triggerBitField, const SupervisorStateTransitionList_t* transitions); +supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBits_t triggerBitField, const SupervisorStateTransitionList_t* transitions); // Helpers -static void assertStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet); -static void assertNoStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet); +static void assertStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet); +static void assertNoStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet); void setUp(void) { // Empty @@ -23,9 +23,9 @@ void tearDown(void) { void testTransitionWithNoConditions(void) { // Fixture - supervisorConditionBit_t conditions = 123; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = 123; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -34,9 +34,9 @@ void testTransitionWithNoConditions(void) { void testTransitionOnePositiveConditionMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -45,9 +45,9 @@ void testTransitionOnePositiveConditionMet(void) { void testTransitionOnePositiveConditionNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -56,9 +56,9 @@ void testTransitionOnePositiveConditionNotMet(void) { void testTransitionMultiPositiveConditionsMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -67,9 +67,9 @@ void testTransitionMultiPositiveConditionsMet(void) { void testTransitionMultiPositiveConditionsMetWithOtherPositives(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED | SUPERVISOR_TB_EMERGENCY_STOP; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -78,9 +78,9 @@ void testTransitionMultiPositiveConditionsMetWithOtherPositives(void) { void testTransitionMultiPositiveConditionsOneMissing(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -89,9 +89,9 @@ void testTransitionMultiPositiveConditionsOneMissing(void) { void testTransitionMultiPositiveConditionsOneMissingButOtherPositives(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_NONE; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; // Test // Assert @@ -100,9 +100,9 @@ void testTransitionMultiPositiveConditionsOneMissingButOtherPositives(void) { void testTransitionOneNegativeConditionMet(void) { // Fixture - supervisorConditionBit_t conditions = 0; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBits_t conditions = 0; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; // Test // Assert @@ -111,9 +111,9 @@ void testTransitionOneNegativeConditionMet(void) { void testTransitionOneNegativeConditionNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; // Test // Assert @@ -122,9 +122,9 @@ void testTransitionOneNegativeConditionNotMet(void) { void testTransitionOneNegativeConditionNotMetWithOtherPositives(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED; + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; // Test // Assert @@ -133,9 +133,9 @@ void testTransitionOneNegativeConditionNotMetWithOtherPositives(void) { void testTransitionMultiNegativeConditionsMet(void) { // Fixture - supervisorConditionBit_t conditions = 0; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBits_t conditions = 0; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; // Test // Assert @@ -144,9 +144,9 @@ void testTransitionMultiNegativeConditionsMet(void) { void testTransitionMultiNegativeConditionsOneNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_NONE; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_IS_TUMBLED; + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; // Test // Assert @@ -155,9 +155,9 @@ void testTransitionMultiNegativeConditionsOneNotMet(void) { void testTransitionMultiPositiveAndNegativeConditionsMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; // Test // Assert @@ -166,9 +166,9 @@ void testTransitionMultiPositiveAndNegativeConditionsMet(void) { void testTransitionMultiPositiveAndNegativeConditionsOnePositiveNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; // Test // Assert @@ -177,9 +177,9 @@ void testTransitionMultiPositiveAndNegativeConditionsOnePositiveNotMet(void) { void testTransitionMultiPositiveAndNegativeConditionsOneNegativeNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; // Test // Assert @@ -188,9 +188,9 @@ void testTransitionMultiPositiveAndNegativeConditionsOneNegativeNotMet(void) { void testTransitionMultiPositiveAndNegativeConditionsMultipleNotMet(void) { // Fixture - supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; - supervisorConditionBit_t mustBeSet = SUPERVISOR_TB_ARMED | SUPERVISOR_TB_IS_TUMBLED; - supervisorConditionBit_t mustNotBeSet = SUPERVISOR_TB_CHARGER_CONNECTED | SUPERVISOR_TB_EMERGENCY_STOP; + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; // Test // Assert @@ -199,7 +199,7 @@ void testTransitionMultiPositiveAndNegativeConditionsMultipleNotMet(void) { void testFirstValidTransitionIsChosen(void) { // Fixture - const supervisorConditionBit_t conditions = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP; + const supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; const supervisorState_t currentState = supervisorStateFlying; const supervisorState_t expected = supervisorStateExceptFreeFall; @@ -207,18 +207,18 @@ void testFirstValidTransitionIsChosen(void) { SupervisorStateTransition_t transitions[] = { { .newState = supervisorStateLanded, - .mustBeSet = SUPERVISOR_TB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_TB_EMERGENCY_STOP + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_CB_EMERGENCY_STOP }, { // We expect this state to be chosen .newState = expected, - .mustBeSet = SUPERVISOR_TB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_CB_NONE }, { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_TB_IS_TUMBLED | SUPERVISOR_TB_EMERGENCY_STOP, - .mustNotBeSet = SUPERVISOR_TB_NONE + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_CB_NONE } }; @@ -233,7 +233,7 @@ void testFirstValidTransitionIsChosen(void) { // Helpers //////////////////////////////////////////////// -static bool check_state_transition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { +static bool check_state_transition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { // Fixture const supervisorState_t currentState = supervisorStateFlying; @@ -254,10 +254,10 @@ static bool check_state_transition(supervisorConditionBit_t conditions, supervis return newState != currentState; } -static void assertStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { +static void assertStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { TEST_ASSERT_TRUE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) } -static void assertNoStateTransition(supervisorConditionBit_t conditions, supervisorConditionBit_t mustBeSet, supervisorConditionBit_t mustNotBeSet) { +static void assertNoStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { TEST_ASSERT_FALSE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) } From 3334fd5caf65b1e217dc9be71e7a1c49e1cba28d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 14:24:23 +0200 Subject: [PATCH 08/30] Similar functionality to master --- docs/functional-areas/supervisor/states.md | 47 ++++++++----------- .../interface/supervisor_state_machine.h | 4 +- src/modules/src/supervisor.c | 36 ++------------ src/modules/src/supervisor_state_machine.c | 34 +++++++++++--- 4 files changed, 52 insertions(+), 69 deletions(-) diff --git a/docs/functional-areas/supervisor/states.md b/docs/functional-areas/supervisor/states.md index a87aeed977..8982ce158e 100644 --- a/docs/functional-areas/supervisor/states.md +++ b/docs/functional-areas/supervisor/states.md @@ -19,33 +19,26 @@ page_id: supervisor_states Arming| | | + Reset + V | | + + +---------+---------+ | +-----------------+ -+ Ready to fly +---+ ^ ^ -+ + | | -+--------+----------+ | | - | | | - V | | -+-------------------+ | | +-------------------+ -+ Flying +------------|-|------>+ Exception + -+ +------------|-|---+ + Free fall +--+ -+--------+----------+ | | | +-------------------+ | - | | | | | - V | | | | -+-------------------+ | | | +-------------------+ | -+ Landed +------------+ | +-->+ Exception + | -+ | | + Soft fall + | -+--------+----------+ | +---------+---------+ | - | | | | - | | | +---------+ - | | | | - | | V V - +---------------+ | +-------------------+ - | +-------+ Platform not + - | + moving + - | +---------+---------+ - | | - | +--------------+ - | | - V V ++ Ready to fly +---+ ^ ++ + | ++--------+----------+ | + | | + V | ++-------------------+ | +-------------------+ ++ Flying +------------|-------->+ Warning + ++ +<-----------|---------+ Level out + ++--------+-------+--+ | +---------+---------+ + | | | | + V +---------------|-----+ V ++-------------------+ | | +-------------------+ ++ Landed +------------+ +-->+ Exception + ++ | + Free fall + ++--------+----------+ +---------+---------+ + | + | + +--------------+ + | + V +-------------------+ + Lock + + + diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h index 5639e75f5d..db653a56ca 100644 --- a/src/modules/interface/supervisor_state_machine.h +++ b/src/modules/interface/supervisor_state_machine.h @@ -35,7 +35,7 @@ typedef enum { supervisorStateReset, supervisorStateWarningLevelOut, supervisorStateExceptFreeFall, - supervisorStateExceptNotMoving, + supervisorStateLocked, supervisorState_NrOfStates, } supervisorState_t; @@ -45,7 +45,6 @@ enum { supervisorConditionChargerConnected, supervisorConditionIsFlying, supervisorConditionIsTumbled, - supervisorConditionIsMoving, supervisorConditionCommanderWdtWarning, supervisorConditionCommanderWdtTimeout, supervisorConditionEmergencyStop, @@ -59,7 +58,6 @@ typedef uint32_t supervisorConditionBits_t; #define SUPERVISOR_CB_CHARGER_CONNECTED (1 << supervisorConditionChargerConnected) #define SUPERVISOR_CB_IS_FLYING (1 << supervisorConditionIsFlying) #define SUPERVISOR_CB_IS_TUMBLED (1 << supervisorConditionIsTumbled) -#define SUPERVISOR_CB_IS_MOVING (1 << supervisorConditionIsMoving) #define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning) #define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout) #define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 1749f9260a..104e318a2c 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -52,13 +52,6 @@ #define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) #define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) -typedef enum { - actionNone = 0, // All is normal, we can fly - actionMotorsDisabled, // Motors are disabled for now, for instance when not armed - actionLevelOut, // Level out, all is not lost yet - actionStopMotorsAndFreeFall, // Stop motors permanently and free fall -} action_t; - typedef struct { bool canFly; bool isFlying; @@ -66,7 +59,6 @@ typedef struct { uint32_t tumbleHysteresis; - action_t action; supervisorState_t state; } SupervisorMem_t; @@ -75,9 +67,6 @@ static SupervisorMem_t supervisorMem; const static setpoint_t nullSetpoint; -// TODO krri -// * Add reset functionality - bool supervisorCanFly() { return supervisorMem.canFly; } @@ -128,11 +117,6 @@ static bool isTumbledCheck(SupervisorMem_t* this, const sensorData_t *data) { return false; } -static bool isMovingCheck(SupervisorMem_t* this, const sensorData_t *data) { - // TODO krri implement - return true; -} - static bool checkEmergencyStopWatchdog(const uint32_t tick) { bool isOk = true; @@ -169,9 +153,6 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { if (this->isTumbled) { conditions |= SUPERVISOR_CB_IS_TUMBLED; } - if (isMovingCheck(this, sensors)) { - conditions |= SUPERVISOR_CB_IS_MOVING; - } const uint32_t setpointAge = currentTick - setpoint->timestamp; if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { conditions |= SUPERVISOR_CB_COMMANDER_WDT_WARNING; @@ -215,19 +196,8 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { // Keep Z as it is break; - case supervisorStateLanded: - // Fall through - case supervisorStateReset: - // Fall through - case supervisorStateExceptFreeFall: - // Fall through - case supervisorStateExceptNotMoving: - // Fall through - case supervisorStatePreFlChecksNotPassed: - // Fall through - case supervisorStatePreFlChecksPassed: - // Fall through default: + // Replace with null setpoint to stop motors memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); break; } @@ -235,7 +205,9 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint) { bool supervisorAreMotorsAllowedToRun() { SupervisorMem_t* this = &supervisorMem; - return (this->action == actionNone) || (this->action == actionLevelOut); + return (this->state == supervisorStateReadyToFly) || + (this->state == supervisorStateFlying) || + (this->state == supervisorStateWarningLevelOut); } /** diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index df93a1e055..2db5f16302 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -43,7 +43,7 @@ static const char* const stateNames[] = { "Reset", "Warning, level out", "Exception, free fall", - "Exception, not moving", + "Locked", }; static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates); #endif @@ -72,6 +72,11 @@ static SupervisorStateTransition_t transitionsReadyToFly[] = { .mustBeSet = SUPERVISOR_CB_NONE, .mustNotBeSet = SUPERVISOR_CB_ARMED }, + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, { .newState = supervisorStateFlying, .mustBeSet = SUPERVISOR_CB_IS_FLYING, @@ -85,6 +90,16 @@ static SupervisorStateTransition_t transitionsFlying[] = { .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, .mustNotBeSet = SUPERVISOR_CB_NONE }, + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, { .newState = supervisorStateWarningLevelOut, .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING, @@ -101,7 +116,7 @@ static SupervisorStateTransition_t transitionsLanded[] = { { .newState = supervisorStateReset, .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_IS_MOVING + .mustNotBeSet = SUPERVISOR_CB_NONE }, }; @@ -119,6 +134,11 @@ static SupervisorStateTransition_t transitionsWarningLevelOut[] = { .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, .mustNotBeSet = SUPERVISOR_CB_NONE }, + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, { .newState = supervisorStateFlying, .mustBeSet = SUPERVISOR_CB_NONE, @@ -128,15 +148,15 @@ static SupervisorStateTransition_t transitionsWarningLevelOut[] = { static SupervisorStateTransition_t transitionsExceptFreeFall[] = { { - .newState = supervisorStateExceptNotMoving, + .newState = supervisorStateLocked, .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_IS_MOVING + .mustNotBeSet = SUPERVISOR_CB_NONE }, }; -static SupervisorStateTransition_t transitionsExceptNotMoving[] = { +static SupervisorStateTransition_t transitionsLocked[] = { { - .newState = supervisorStateReset, + .newState = supervisorStateLocked, .mustBeSet = SUPERVISOR_CB_NONE, .mustNotBeSet = SUPERVISOR_CB_NONE }, @@ -151,7 +171,7 @@ SupervisorStateTransitionList_t transitionLists[] = { {SUPERVISOR_TRANSITION_ENTRY(transitionsReset)}, {SUPERVISOR_TRANSITION_ENTRY(transitionsWarningLevelOut)}, {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptFreeFall)}, - {SUPERVISOR_TRANSITION_ENTRY(transitionsExceptNotMoving)}, + {SUPERVISOR_TRANSITION_ENTRY(transitionsLocked)}, }; static_assert(sizeof(transitionLists) / sizeof(transitionLists[0]) == supervisorState_NrOfStates); From efcce28247ddac3fe7b6027b8ef03b908597f359 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 14:38:07 +0200 Subject: [PATCH 09/30] Free fall if disarmed when flying --- src/modules/src/supervisor_state_machine.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 2db5f16302..87d607d2bc 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -100,6 +100,11 @@ static SupervisorStateTransition_t transitionsFlying[] = { .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, .mustNotBeSet = SUPERVISOR_CB_NONE }, + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_NONE, + .mustNotBeSet = SUPERVISOR_CB_ARMED + }, { .newState = supervisorStateWarningLevelOut, .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING, From b1b74753d378eb78478dbf3f197f10c628e985ab Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 26 Apr 2023 14:42:46 +0200 Subject: [PATCH 10/30] Updated arming system which is on by default for bolt platform. ForceArm has now been changed, its parameter renamed to arm and the persistance has been removed. It has also been made CORE. Now there is only one variable "arm" which can be changed by the system or parameter. The parameter update was also fixed so it only sends notifications if a client is connected. --- configs/bolt_defconfig | 2 ++ src/modules/src/Kconfig | 8 +++--- src/modules/src/param_logic.c | 48 ++++++++++++++++++++--------------- src/modules/src/system.c | 24 ++++++++++-------- 4 files changed, 47 insertions(+), 35 deletions(-) diff --git a/configs/bolt_defconfig b/configs/bolt_defconfig index c30ba1a53c..2a4e1534e4 100644 --- a/configs/bolt_defconfig +++ b/configs/bolt_defconfig @@ -2,3 +2,5 @@ CONFIG_PLATFORM_BOLT=y CONFIG_ESTIMATOR_AUTO_SELECT=y CONFIG_CONTROLLER_AUTO_SELECT=y + +CONFIG_MOTORS_REQUIRE_ARMING=y \ No newline at end of file diff --git a/src/modules/src/Kconfig b/src/modules/src/Kconfig index 2afe902398..a8b3712f38 100644 --- a/src/modules/src/Kconfig +++ b/src/modules/src/Kconfig @@ -143,12 +143,12 @@ config MOTORS_DSHOT_PWM_600KHZ endchoice -config MOTORS_START_DISARMED - bool "Set disarmed state after boot" +config MOTORS_REQUIRE_ARMING + bool "Require arming to be able to start motors and take off" default n help - When enabled, the firmware will boot in disarmed state and one needs to - arm the drone explicitly before starting the motors + When enabled, system must be armed to be able to take off. Arming can + be done in several ways, e.g. though cfclient or external transmitter. config MOTORS_DEFAULT_IDLE_THRUST int "Default idle thrust for motors in armed state" diff --git a/src/modules/src/param_logic.c b/src/modules/src/param_logic.c index 9a6d47038e..8a84e9f780 100644 --- a/src/modules/src/param_logic.c +++ b/src/modules/src/param_logic.c @@ -571,15 +571,19 @@ void paramSetInt(paramVarId_t varid, int valuei) paramSize = paramSet(varid.index, (void *)&valuei); #ifndef CONFIG_PARAM_SILENT_UPDATES - static CRTPPacket pk; - pk.header=CRTP_HEADER(CRTP_PORT_PARAM, MISC_CH); - pk.data[0] = MISC_VALUE_UPDATED; - pk.data[1] = varid.id & 0xffu; - pk.data[2] = (varid.id >> 8) & 0xffu; - pk.size = 3 + paramSize; - const int sendResult = crtpSendPacket(&pk); - if (sendResult == errQUEUE_FULL) { - DEBUG_PRINT("WARNING: Param update not sent\n"); + if (crtpIsConnected()) + { + static CRTPPacket pk; + pk.header=CRTP_HEADER(CRTP_PORT_PARAM, MISC_CH); + pk.data[0] = MISC_VALUE_UPDATED; + pk.data[1] = varid.id & 0xffu; + pk.data[2] = (varid.id >> 8) & 0xffu; + pk.size = 3 + paramSize; + const int sendResult = crtpSendPacket(&pk); + if (sendResult == errQUEUE_FULL) + { + DEBUG_PRINT("WARNING: Param update not sent\n"); + } } #endif @@ -594,17 +598,21 @@ void paramSetFloat(paramVarId_t varid, float valuef) *(float *)params[varid.index].address = valuef; #ifndef CONFIG_PARAM_SILENT_UPDATES - static CRTPPacket pk; - pk.header = CRTP_HEADER(CRTP_PORT_PARAM, MISC_CH); - pk.data[0] = MISC_VALUE_UPDATED; - pk.data[1] = varid.id & 0xffu; - pk.data[2] = (varid.id >> 8) & 0xffu; - pk.size = 3; - memcpy(&pk.data[2], &valuef, 4); - pk.size += 4; - const int sendResult = crtpSendPacket(&pk); - if (sendResult == errQUEUE_FULL) { - DEBUG_PRINT("WARNING: Param update not sent\n"); + if (crtpIsConnected()) + { + static CRTPPacket pk; + pk.header = CRTP_HEADER(CRTP_PORT_PARAM, MISC_CH); + pk.data[0] = MISC_VALUE_UPDATED; + pk.data[1] = varid.id & 0xffu; + pk.data[2] = (varid.id >> 8) & 0xffu; + pk.size = 3; + memcpy(&pk.data[2], &valuef, 4); + pk.size += 4; + const int sendResult = crtpSendPacket(&pk); + if (sendResult == errQUEUE_FULL) + { + DEBUG_PRINT("WARNING: Param update not sent\n"); + } } #endif diff --git a/src/modules/src/system.c b/src/modules/src/system.c index 4b171048ac..d7cecc2109 100644 --- a/src/modules/src/system.c +++ b/src/modules/src/system.c @@ -78,16 +78,15 @@ #include "cpxlink.h" #endif -#ifndef CONFIG_MOTORS_START_DISARMED -#define ARM_INIT true +#ifndef CONFIG_MOTORS_REQUIRE_ARMING + #define ARMING_REQUIRED true #else -#define ARM_INIT false + #define ARMING_REQUIRED false #endif /* Private variable */ static bool selftestPassed; -static bool armed = ARM_INIT; -static bool forceArm; +static bool arm = ARMING_REQUIRED; static uint8_t dumpAssertInfo = 0; static bool isInit; @@ -363,13 +362,16 @@ void systemWaitStart(void) void systemSetArmed(bool val) { - armed = val; + if (arm != val) { + // Set using parameter to update any client + paramVarId_t armId = paramGetVarId("system", "arm"); + paramSetInt(armId, val); + } } bool systemIsArmed() { - - return armed || forceArm; + return arm; } void systemRequestShutdown() @@ -466,9 +468,9 @@ PARAM_GROUP_START(system) PARAM_ADD_CORE(PARAM_INT8 | PARAM_RONLY, selftestPassed, &selftestPassed) /** - * @brief Set to nonzero to force system to be armed + * @brief Set to nonzero to arm the system */ -PARAM_ADD(PARAM_INT8 | PARAM_PERSISTENT, forceArm, &forceArm) +PARAM_ADD_CORE(PARAM_INT8, arm, &arm) /** * @brief Set to nonzero to trigger dump of assert information to the log. @@ -490,7 +492,7 @@ LOG_GROUP_START(sys) /** * @brief If zero, arming system is preventing motors to start */ -LOG_ADD(LOG_INT8, armed, &armed) +LOG_ADD(LOG_INT8, armed, &arm) /** * @brief Test util for log and param. The value is set through the system.testLogParam parameter From 65cebc2c66f5546a7cb81b4b729b2abb0d689821 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 15:32:41 +0200 Subject: [PATCH 11/30] Enabled param stabilizer.stop again --- src/modules/src/supervisor.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 104e318a2c..7bac0b78f5 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -56,6 +56,7 @@ typedef struct { bool canFly; bool isFlying; bool isTumbled; + uint8_t paramEmergencyStop; uint32_t tumbleHysteresis; @@ -166,6 +167,9 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { if (locSrvIsEmergencyStopRequested()) { conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } + if (this->paramEmergencyStop) { + conditions |= SUPERVISOR_CB_EMERGENCY_STOP; + } supervisorState_t newState = supervisorStateUpdate(this->state, conditions); @@ -234,7 +238,5 @@ PARAM_GROUP_START(stabilizer) /** * @brief If set to nonzero will turn off power */ - -// TODO krri How to handle? -// PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.areMotorsLocked) +PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.paramEmergencyStop) PARAM_GROUP_STOP(stabilizer) From b574b676813116f88b780c3fb7cd8d3e8e913174 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 15:57:07 +0200 Subject: [PATCH 12/30] Use explicit time for isTumbledCheck() --- src/modules/src/supervisor.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 7bac0b78f5..0ba0c63609 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -45,8 +45,8 @@ /* Minimum summed motor PWM that means we are flying */ #define SUPERVISOR_FLIGHT_THRESHOLD 1000 -/* Number of times in a row we need to see a condition before acting upon it */ -#define TUMBLE_HYSTERESIS_THRESHOLD 30 +/* The minimum time (in ms) we need to see tumble condition before acting on it */ +#define TUMBLE_HYSTERESIS_THRESHOLD M2T(30) // TODO krri rename #define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) @@ -58,7 +58,8 @@ typedef struct { bool isTumbled; uint8_t paramEmergencyStop; - uint32_t tumbleHysteresis; + // The time (in ticks) of the first tumble event. 0=no tumble + uint32_t initialTumbleTick; supervisorState_t state; } SupervisorMem_t; @@ -100,19 +101,21 @@ static bool isFlyingCheck() { // the thrust to the motors, avoiding the Crazyflie from running propellers at // significant thrust when accidentally crashing into walls or the ground. // -static bool isTumbledCheck(SupervisorMem_t* this, const sensorData_t *data) { +static bool isTumbledCheck(SupervisorMem_t* this, const sensorData_t *data, const uint32_t tick) { const float tolerance = -0.5; // // We need a TUMBLE_HYSTERESIS_THRESHOLD amount of readings that indicate // that we are tumbled before we act on it. This is to reduce false positives. // if (data->acc.z <= tolerance) { - this->tumbleHysteresis++; - if (this->tumbleHysteresis > TUMBLE_HYSTERESIS_THRESHOLD) { + if (0 == this->initialTumbleTick) { + this->initialTumbleTick = tick; + } + if ((tick - this->initialTumbleTick) > TUMBLE_HYSTERESIS_THRESHOLD) { return true; } } else { - this->tumbleHysteresis = 0; + this->initialTumbleTick = 0; } return false; @@ -135,7 +138,7 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { const uint32_t currentTick = xTaskGetTickCount(); this->isFlying = isFlyingCheck(); - this->isTumbled = isTumbledCheck(this, sensors); + this->isTumbled = isTumbledCheck(this, sensors, currentTick); // canFly is kept for backwards compatibility. TODO krri deprecate? this->canFly = true; From 08fad134bcf26fe46067452d5019c8b3fcd0ea76 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Wed, 26 Apr 2023 15:59:19 +0200 Subject: [PATCH 13/30] Fixed param_logic tests after isConnected update --- test/modules/src/test_param_logic.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/test/modules/src/test_param_logic.c b/test/modules/src/test_param_logic.c index 1de9deec36..c378ddcde1 100644 --- a/test/modules/src/test_param_logic.c +++ b/test/modules/src/test_param_logic.c @@ -83,6 +83,7 @@ void testSetUint8(void) { // Fixture uint8_t expected = UINT8_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myUint8"); @@ -97,6 +98,7 @@ void testSetUint16(void) { // Fixture uint16_t expected = UINT16_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myUint16"); @@ -111,6 +113,7 @@ void testSetUint32(void) { // Fixture uint32_t expected = UINT32_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myUint32"); @@ -125,6 +128,7 @@ void testSetInt8(void) { // Fixture uint8_t expected = INT8_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myInt8"); @@ -139,6 +143,7 @@ void testSetInt16(void) { // Fixture uint16_t expected =UINT16_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myInt16"); @@ -153,6 +158,7 @@ void testSetInt32(void) { // Fixture uint32_t expected = INT32_MAX - 1; + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); paramVarId_t varid = paramGetVarId("myGroup", "myInt32"); @@ -268,6 +274,7 @@ void testPersistentSetGetFloat(void) { paramVarId_t varid = paramGetVarId("myGroup", "myPersistentFloat"); + crtpIsConnected_IgnoreAndReturn(0); crtpSendPacket_StubWithCallback(crtpReply); // Test From 4519c8e3352acb10bca6c458bf673b2f8fe9c5c4 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 16:05:33 +0200 Subject: [PATCH 14/30] Run supervisor at 25Hz --- src/modules/src/stabilizer.c | 2 +- src/modules/src/supervisor.c | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 2e146fa912..283b3b3930 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -287,7 +287,7 @@ static void stabilizerTask(void* param) // Critical for safety, be careful if you modify this code! // Let the supervisor update it's view of the current situation - supervisorUpdate(&sensorData, &setpoint); + supervisorUpdate(&sensorData, &setpoint, stabilizerStep); // Let the collision avoidance module modify the setpoint, if needed collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep); diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 0ba0c63609..9949a1594a 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -133,7 +133,11 @@ static bool checkEmergencyStopWatchdog(const uint32_t tick) { } -void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint) { +void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) { + if (!RATE_DO_EXECUTE(RATE_SUPERVISOR, stabilizerStep)) { + return; + } + SupervisorMem_t* this = &supervisorMem; const uint32_t currentTick = xTaskGetTickCount(); From 9596af43f75239ae61db2a245bcd348bdadd18fb Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 26 Apr 2023 16:14:46 +0200 Subject: [PATCH 15/30] Added missing files --- src/modules/interface/stabilizer_types.h | 3 ++- src/modules/interface/supervisor.h | 2 +- src/modules/src/supervisor.c | 11 ++++------- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index f0ff1d9302..d49f852b6a 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -356,7 +356,7 @@ typedef struct } barometerMeasurement_t; -// Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. +// Frequencies to be used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. #define RATE_1000_HZ 1000 #define RATE_500_HZ 500 #define RATE_250_HZ 250 @@ -368,6 +368,7 @@ typedef struct #define ATTITUDE_RATE RATE_500_HZ #define POSITION_RATE RATE_100_HZ #define RATE_HL_COMMANDER RATE_100_HZ +#define RATE_SUPERVISOR RATE_25_HZ #define RATE_DO_EXECUTE(RATE_HZ, TICK) ((TICK % (RATE_MAIN_LOOP / RATE_HZ)) == 0) diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index 9742a061f4..e08c74e4ed 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -28,7 +28,7 @@ #include "stabilizer_types.h" -void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint); +void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep); void supervisorOverrideSetpoint(setpoint_t* setpoint); bool supervisorAreMotorsAllowedToRun(); diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 9949a1594a..98372330b7 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -48,7 +48,6 @@ /* The minimum time (in ms) we need to see tumble condition before acting on it */ #define TUMBLE_HYSTERESIS_THRESHOLD M2T(30) -// TODO krri rename #define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) #define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) @@ -144,10 +143,6 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s this->isFlying = isFlyingCheck(); this->isTumbled = isTumbledCheck(this, sensors, currentTick); - // canFly is kept for backwards compatibility. TODO krri deprecate? - this->canFly = true; - - supervisorConditionBits_t conditions = 0; if (systemIsArmed()) { conditions |= SUPERVISOR_CB_ARMED; @@ -180,7 +175,9 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s supervisorState_t newState = supervisorStateUpdate(this->state, conditions); - // TODO krri add transition actions? + this->canFly = supervisorAreMotorsAllowedToRun(); + + // Transition actions can be added here if needed this->state = newState; } @@ -221,6 +218,7 @@ bool supervisorAreMotorsAllowedToRun() { (this->state == supervisorStateWarningLevelOut); } +// TODO krri Do we want to deprecate any of these? /** * System loggable variables to check different system states. */ @@ -240,7 +238,6 @@ LOG_ADD_CORE(LOG_UINT8, isTumbled, &supervisorMem.isTumbled) LOG_GROUP_STOP(sys) -// TODO Krri rename? deprecate? PARAM_GROUP_START(stabilizer) /** * @brief If set to nonzero will turn off power From 6b357753b989f15e6e2ee632aa9156aa74f2ea71 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 13:29:30 +0200 Subject: [PATCH 16/30] Added timeout for setting isFlying to false --- src/modules/interface/power_distribution.h | 7 +++ src/modules/src/power_distribution_flapper.c | 4 ++ .../src/power_distribution_quadrotor.c | 4 ++ src/modules/src/supervisor.c | 43 ++++++++++++++----- 4 files changed, 47 insertions(+), 11 deletions(-) diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h index 1577ff1983..4489b5d524 100644 --- a/src/modules/interface/power_distribution.h +++ b/src/modules/interface/power_distribution.h @@ -60,4 +60,11 @@ int powerDistributionMotorType(uint32_t id); */ uint16_t powerDistributionStopRatio(uint32_t id); +/** + * @brief Get the current setting for idle thrust + * + * @return uint32_t The idle thrust + */ +uint32_t powerDistributionGetIdleThrust(); + #endif //__POWER_DISTRIBUTION_H__ diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c index 495d72b4ab..9cf98a4c05 100644 --- a/src/modules/src/power_distribution_flapper.c +++ b/src/modules/src/power_distribution_flapper.c @@ -189,6 +189,10 @@ void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUnca #endif } +uint32_t powerDistributionGetIdleThrust() { + return idleThrust; +} + /** * Power distribution parameters */ diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c index cda7d8ccfa..ef38614251 100644 --- a/src/modules/src/power_distribution_quadrotor.c +++ b/src/modules/src/power_distribution_quadrotor.c @@ -162,6 +162,10 @@ void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUnca } } +uint32_t powerDistributionGetIdleThrust() { + return idleThrust; +} + /** * Power distribution parameters */ diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 98372330b7..7a5edf5eb1 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -42,12 +42,12 @@ #define DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT (M2T(1000)) -/* Minimum summed motor PWM that means we are flying */ -#define SUPERVISOR_FLIGHT_THRESHOLD 1000 - -/* The minimum time (in ms) we need to see tumble condition before acting on it */ +// The minimum time (in ms) we need to see tumble condition before acting on it #define TUMBLE_HYSTERESIS_THRESHOLD M2T(30) +// The minimum time (in ms) we need to see low thrust before saying that we are not flying anymore +#define IS_FLYING_HYSTERESIS_THRESHOLD M2T(2000) + #define COMMANDER_WDT_TIMEOUT_STABILIZE M2T(500) #define COMMANDER_WDT_TIMEOUT_SHUTDOWN M2T(2000) @@ -60,6 +60,9 @@ typedef struct { // The time (in ticks) of the first tumble event. 0=no tumble uint32_t initialTumbleTick; + // The time (in ticks) of the first low thrust event, when flying. 0=no low thrust event + uint32_t initialLowThrustTick; + supervisorState_t state; } SupervisorMem_t; @@ -81,16 +84,34 @@ bool supervisorIsTumbled() { } // -// We say we are flying if the sum of the ratios of all motors giving thrust -// is above a certain threshold. +// We say we are flying if one or more motors are running over the idle thrust. // -static bool isFlyingCheck() { - int sumRatio = 0; +static bool isFlyingCheck(SupervisorMem_t* this, const uint32_t tick) { + bool result = false; + + bool isThrustOverIdle = false; + const uint32_t idleThrust = powerDistributionGetIdleThrust(); for (int i = 0; i < NBR_OF_MOTORS; ++i) { - sumRatio += powerDistributionMotorType(i) * motorsGetRatio(i); + const uint32_t ratio = powerDistributionMotorType(i) * motorsGetRatio(i); + if (ratio > idleThrust) { + isThrustOverIdle = true; + break; + } + } + + if (isThrustOverIdle) { + this->initialLowThrustTick = 0; + result = true; + } else { + if (0 == this->initialLowThrustTick) { + this->initialLowThrustTick = tick; + } + if ((tick - this->initialLowThrustTick) < IS_FLYING_HYSTERESIS_THRESHOLD) { + result = true; + } } - return sumRatio > SUPERVISOR_FLIGHT_THRESHOLD; + return result; } // @@ -140,7 +161,7 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s SupervisorMem_t* this = &supervisorMem; const uint32_t currentTick = xTaskGetTickCount(); - this->isFlying = isFlyingCheck(); + this->isFlying = isFlyingCheck(this, currentTick); this->isTumbled = isTumbledCheck(this, sensors, currentTick); supervisorConditionBits_t conditions = 0; From f036e45eb3d6b3154720678e354cd5a88826be6d Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 13:41:48 +0200 Subject: [PATCH 17/30] Add blocking of High level commander by supervisor --- .../interface/crtp_commander_high_level.h | 28 ++++++++- src/modules/interface/planner.h | 2 +- src/modules/src/crtp_commander_high_level.c | 63 ++++++++++++++++++- src/modules/src/stabilizer.c | 7 ++- 4 files changed, 96 insertions(+), 4 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index cdc16b30a0..cb6baba14f 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -69,7 +69,7 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat // commander what initial conditions to use for trajectory planning. void crtpCommanderHighLevelTellState(const state_t *state); -// True if we have landed or emergency-stopped. +// True if we have landed or stopped. bool crtpCommanderHighLevelIsStopped(); // Public API - can be used from an app @@ -158,6 +158,32 @@ int crtpCommanderHighLevelStop(); */ int crtpCommanderHighLevelDisable(); +/** + * @brief Block/unblock the use of the high level commander. + * + * This function is called from the stabilizer loop. The purpose is to provide a way for the supervisor to block a user + * (or app) from starting a trajectory when the system is not in a flyable state. + * + * When entering the blocked state, the planer will stop any running trajectory and go to the stopped state. If + * the planner already is in the stopped or disabled state, it will remain. + * + * When blocked, functions that plans a new trajectory will be blocked. + * + * @param doBlock Enter blocked state if true, unblock if false + * @return zero if the command succeeded, an error code otherwise. The function + * should never fail, but we provide the error code nevertheless for sake of + * consistency with the other high-level commander functions. + */ +int crtpCommanderBlock(bool doBlock); + +/** + * @brief Check if the high level commander is blocked by the supervisor + * + * @return true If the high level commander is blocked by the supervisor + * @return false If not blocked + */ +bool crtpCommanderHighLevelIsBlocked(); + /** * @brief Go to an absolute or relative position * diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h index 2842263899..c242a9390c 100644 --- a/src/modules/interface/planner.h +++ b/src/modules/interface/planner.h @@ -87,7 +87,7 @@ void plan_stop(struct planner *p); // query if the planner is stopped. // currently this is true at startup before we take off, -// and also after an emergency stop. +// and also after a stop. bool plan_is_stopped(struct planner *p); // disable the planner. diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 3a3a8758cc..ee540169e9 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -97,6 +97,7 @@ const static setpoint_t nullSetpoint; static bool isInit = false; static struct planner planner; static uint8_t group_mask; +static bool isBlocked; // Are we blocked to do anything by the supervisor static struct vec pos; // last known setpoint (position [m]) static struct vec vel; // last known setpoint (velocity [m/s]) static float yaw; // last known setpoint yaw (yaw [rad]) @@ -275,6 +276,8 @@ void crtpCommanderHighLevelInit(void) vel = vzero(); yaw = 0; + isBlocked = false; + isInit = true; } @@ -436,6 +439,10 @@ int set_group_mask(const struct data_set_group_mask* data) // Deprecated (removed after August 2023) int takeoff(const struct data_takeoff* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -448,6 +455,10 @@ int takeoff(const struct data_takeoff* data) int takeoff2(const struct data_takeoff_2* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -466,6 +477,10 @@ int takeoff2(const struct data_takeoff_2* data) int takeoff_with_velocity(const struct data_takeoff_with_velocity* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -492,6 +507,10 @@ int takeoff_with_velocity(const struct data_takeoff_with_velocity* data) // Deprecated (removed after August 2023) int land(const struct data_land* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -504,6 +523,10 @@ int land(const struct data_land* data) int land2(const struct data_land_2* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -522,6 +545,10 @@ int land2(const struct data_land_2* data) int land_with_velocity(const struct data_land_with_velocity* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { xSemaphoreTake(lockTraj, portMAX_DELAY); @@ -564,6 +591,10 @@ int go_to(const struct data_go_to* data) .omega = {0.0f, 0.0f, 0.0f}, }; + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { struct vec hover_pos = mkvec(data->x, data->y, data->z); @@ -585,6 +616,10 @@ int go_to(const struct data_go_to* data) int start_trajectory(const struct data_start_trajectory* data) { + if (isBlocked) { + return EBUSY; + } + int result = 0; if (isInGroup(data->groupMask)) { if (data->trajectoryId < NUM_TRAJECTORY_DEFINITIONS) { @@ -615,7 +650,6 @@ int start_trajectory(const struct data_start_trajectory* data) result = plan_start_compressed_trajectory(&planner, &compressed_trajectory, data->relative, pos); xSemaphoreGive(lockTraj); } - } } } @@ -738,6 +772,33 @@ int crtpCommanderHighLevelStop() return handleCommand(COMMAND_STOP, (const uint8_t*)&data); } +int crtpCommanderBlock(bool doBlock) +{ + if (doBlock) + { + if (!isBlocked) + { + const bool isNotDisabled = !plan_is_disabled(&planner); + const bool isNotStopped = !plan_is_stopped(&planner); + if (isNotDisabled && isNotStopped) + { + xSemaphoreTake(lockTraj, portMAX_DELAY); + plan_stop(&planner); + xSemaphoreGive(lockTraj); + } + } + } + + isBlocked = doBlock; + + return 0; +} + +bool crtpCommanderHighLevelIsBlocked() +{ + return isBlocked; +} + int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative) { struct data_go_to data = diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 283b3b3930..09d8aff3fb 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -280,6 +280,11 @@ static void stabilizerTask(void* param) stateEstimator(&state, stabilizerStep); + const bool areMotorsAllowedToRun = supervisorAreMotorsAllowedToRun(); + + // Critical for safety, be careful if you modify this code! + crtpCommanderBlock(! areMotorsAllowedToRun); + if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, stabilizerStep)) { commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL); } @@ -300,7 +305,7 @@ static void stabilizerTask(void* param) // Critical for safety, be careful if you modify this code! // The supervisor will already set thrust to 0 in the setpoint if needed, but to be extra sure prevent motors from running. - if (supervisorAreMotorsAllowedToRun()) { + if (areMotorsAllowedToRun) { controlMotors(&control); } else { motorsStop(); From 266315742d5477f81cc10eeadeb84f94f0c4f03a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 13:52:53 +0200 Subject: [PATCH 18/30] Added more conditions for leaving ready to fly state --- src/modules/src/supervisor_state_machine.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 87d607d2bc..6163eb4870 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -67,14 +67,24 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { }; static SupervisorStateTransition_t transitionsReadyToFly[] = { + { + .newState = supervisorStateExceptFreeFall, + .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, { .newState = supervisorStatePreFlChecksNotPassed, .mustBeSet = SUPERVISOR_CB_NONE, .mustNotBeSet = SUPERVISOR_CB_ARMED }, { - .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, .mustNotBeSet = SUPERVISOR_CB_NONE }, { From a7835f7276ff3742ddcf13ede6fe2ab181a73404 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 14:20:55 +0200 Subject: [PATCH 19/30] Updated supervisor logging in console --- src/modules/src/supervisor.c | 29 +++++++++++++++++----- src/modules/src/supervisor_state_machine.c | 2 +- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 7a5edf5eb1..23f310960a 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -39,6 +39,9 @@ #include "crtp_localization_service.h" #include "system.h" +#define DEBUG_MODULE "Sup" +#include "debug.h" + #define DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT (M2T(1000)) @@ -152,6 +155,20 @@ static bool checkEmergencyStopWatchdog(const uint32_t tick) { return isOk; } +static void transitionActions(const supervisorState_t currentState, const supervisorState_t newState) { + if (newState == supervisorStateReadyToFly) { + DEBUG_PRINT("Ready to fly\n"); + } + + if (newState == supervisorStateLocked) { + DEBUG_PRINT("Locked, reboot required\n"); + } + + if ((currentState == supervisorStateReadyToFly || currentState == supervisorStateFlying) && + newState != supervisorStateReadyToFly && newState != supervisorStateFlying) { + DEBUG_PRINT("Can not fly\n"); + } +} void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) { if (!RATE_DO_EXECUTE(RATE_SUPERVISOR, stabilizerStep)) { @@ -194,13 +211,13 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } - supervisorState_t newState = supervisorStateUpdate(this->state, conditions); + const supervisorState_t newState = supervisorStateUpdate(this->state, conditions); + if (this->state != newState) { + transitionActions(this->state, newState); + } + this->state = newState; this->canFly = supervisorAreMotorsAllowedToRun(); - - // Transition actions can be added here if needed - - this->state = newState; } @@ -245,7 +262,7 @@ bool supervisorAreMotorsAllowedToRun() { */ LOG_GROUP_START(sys) /** - * @brief If nonzero if system is ready to fly. + * @brief Nonzero if system is ready to fly. */ LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorMem.canFly) /** diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 6163eb4870..e44791ee5b 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -27,7 +27,7 @@ #include "supervisor_state_machine.h" #include "test_support.h" -#define DEBUG_ME +// #define DEBUG_ME #ifdef DEBUG_ME #define DEBUG_MODULE "SupSt" From 235181597fa60a71ce4fc238e8eb8151d6ee03b7 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 14:52:47 +0200 Subject: [PATCH 20/30] Refactoring --- src/modules/src/supervisor.c | 52 ++++++++++++++-------- src/modules/src/supervisor_state_machine.c | 2 +- 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 23f310960a..247695238c 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -39,7 +39,7 @@ #include "crtp_localization_service.h" #include "system.h" -#define DEBUG_MODULE "Sup" +#define DEBUG_MODULE "SUP" #include "debug.h" @@ -170,30 +170,27 @@ static void transitionActions(const supervisorState_t currentState, const superv } } -void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) { - if (!RATE_DO_EXECUTE(RATE_SUPERVISOR, stabilizerStep)) { - return; - } - - SupervisorMem_t* this = &supervisorMem; - const uint32_t currentTick = xTaskGetTickCount(); - - this->isFlying = isFlyingCheck(this, currentTick); - this->isTumbled = isTumbledCheck(this, sensors, 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; } - if (this->isFlying) { + + const bool isFlying = isFlyingCheck(this, currentTick); + if (isFlying) { conditions |= SUPERVISOR_CB_IS_FLYING; } - if (this->isTumbled) { + + const bool isTumbled = isTumbledCheck(this, sensors, currentTick); + if (isTumbled) { conditions |= SUPERVISOR_CB_IS_TUMBLED; } + const uint32_t setpointAge = currentTick - setpoint->timestamp; if (setpointAge > COMMANDER_WDT_TIMEOUT_STABILIZE) { conditions |= SUPERVISOR_CB_COMMANDER_WDT_WARNING; @@ -201,26 +198,46 @@ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, s if (setpointAge > COMMANDER_WDT_TIMEOUT_SHUTDOWN) { conditions |= SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT; } + if (!checkEmergencyStopWatchdog(currentTick)) { conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } + if (locSrvIsEmergencyStopRequested()) { conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } + if (this->paramEmergencyStop) { conditions |= SUPERVISOR_CB_EMERGENCY_STOP; } + return conditions; +} + +static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t conditions) { + this->canFly = supervisorAreMotorsAllowedToRun(); + this->isFlying = (bool)(conditions & SUPERVISOR_CB_IS_FLYING); + this->isTumbled = (bool)(conditions & SUPERVISOR_CB_IS_TUMBLED); +} + +void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) { + if (!RATE_DO_EXECUTE(RATE_SUPERVISOR, stabilizerStep)) { + return; + } + + SupervisorMem_t* this = &supervisorMem; + const uint32_t currentTick = xTaskGetTickCount(); + + 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->state = newState; - this->canFly = supervisorAreMotorsAllowedToRun(); + updateLogData(this, conditions); } - void supervisorOverrideSetpoint(setpoint_t* setpoint) { SupervisorMem_t* this = &supervisorMem; switch(this->state){ @@ -256,7 +273,6 @@ bool supervisorAreMotorsAllowedToRun() { (this->state == supervisorStateWarningLevelOut); } -// TODO krri Do we want to deprecate any of these? /** * System loggable variables to check different system states. */ diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index e44791ee5b..82f19ac509 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -30,7 +30,7 @@ // #define DEBUG_ME #ifdef DEBUG_ME -#define DEBUG_MODULE "SupSt" +#define DEBUG_MODULE "SUPST" #include "debug.h" #include "cfassert.h" From 71b89690408f595e64ff1b76c7266fa72295ab80 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 16:05:06 +0200 Subject: [PATCH 21/30] Supervisor documentation --- .../functional-areas/supervisor/conditions.md | 12 +++++ docs/functional-areas/supervisor/index.md | 26 +++++++++++ docs/functional-areas/supervisor/states.md | 16 +++++++ src/modules/interface/supervisor.h | 46 +++++++++++++++++++ 4 files changed, 100 insertions(+) create mode 100644 docs/functional-areas/supervisor/conditions.md diff --git a/docs/functional-areas/supervisor/conditions.md b/docs/functional-areas/supervisor/conditions.md new file mode 100644 index 0000000000..1b6dc0509d --- /dev/null +++ b/docs/functional-areas/supervisor/conditions.md @@ -0,0 +1,12 @@ +--- +title: Supervisor conditions +page_id: supervisor_conditions +--- + +A condition express the state of some part of the system, for instance if we are flying, if the system is armed or +tumbled. A condition is a single bit and can thus only be true or false. + +All conditions are collected in a bit field that expresses the full state of the system, or at least all the parts +that are relevant to the supervisor. + +All conditions can be found in the `src/modules/interface/supervisor_state_machine.h` file. diff --git a/docs/functional-areas/supervisor/index.md b/docs/functional-areas/supervisor/index.md index be17d57cf5..58197f900a 100644 --- a/docs/functional-areas/supervisor/index.md +++ b/docs/functional-areas/supervisor/index.md @@ -6,4 +6,30 @@ page_id: supervisor_index 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. +The supervisor is based on a state machine that is driven from the supervisor loop. It is given the opportunity +to modify other modules and data, such as the High level commander, setpoints, and motor control, to handle exceptional +situations or for protection. + +## The update sequence + +The update sequence is separated into a few steps: +1. Collect data from sensors and the system. We call these [conditions](conditions.md). +2. Based on the conditions, check if the state machine should transition into a new [state](states.md) +3. If there is a state transition, possibly execute one ore more actions +4. Set the new state + +## Modifying behavior + +The main modification of behavior is to prevent the motors from spinning when the system is not ready to fly, for +instance if the system is not armed or the USB cable is connected. Also the high level commander is blocked from +running trajectories in this case. + +Exceptional situations when flying, for instance when tumbling is detected, are simply handled by stopping the +motors and free falling. + +The supervisor framework provides the possibility to handle situations in a more "clever" way, such as doing a controlled +landing when possible, instead of free falling, but that is currently not implemented. + +## Sub pages + {% sub_page_menu %} diff --git a/docs/functional-areas/supervisor/states.md b/docs/functional-areas/supervisor/states.md index 8982ce158e..9b22c01b51 100644 --- a/docs/functional-areas/supervisor/states.md +++ b/docs/functional-areas/supervisor/states.md @@ -2,6 +2,8 @@ title: Supervisor states page_id: supervisor_states --- + +## State diagram {% ditaa --alt "Supervisor states" %} Boot| V @@ -44,3 +46,17 @@ page_id: supervisor_states + + +-------------------+ {% endditaa %} + +All states can be found in the `src/modules/interface/supervisor_state_machine.h` file. + +## State transitions + +Transitions between states are expressed as a struct containing the next state and two bit fields with requirements for +the transition to be valid. One bitfield contains the [conditions](conditions.md) that **must** be fulfilled and the +other the [conditions](conditions.md) that **must not** be fulfilled, all other [conditions](conditions.md) are ignored. + +For each state there is a list of possible state transitions. The supervisor goes through the list for the current state +and checks if the current [conditions](conditions.md) match the bitfields of the transitions. The first state transition +that matches is used to change state of the supervisor. If no valid transition is found, the state is not changed. + +State transitions are defined in `src/modules/src/supervisor_state_machine.c` diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index e08c74e4ed..be64e44744 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -28,11 +28,57 @@ #include "stabilizer_types.h" +/** + * @brief Update the supervisor state. + * + * The overall process includes: + * - collecting data from the system (conditions) + * - possibly change state, based on the conditions + * + * @param sensors Latest sensor data + * @param setpoint Current setpoint + * @param stabilizerStep Stabilizer step for rate control + */ void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep); +/** + * @brief Replace the values in the current setpoint, if required. + * + * If the supervisor thinks it is necessary to take precautions, for instance in case of an emergency stop, + * if may replace values in the current setpoint. + * + * @param setpoint The current setpoint + */ void supervisorOverrideSetpoint(setpoint_t* setpoint); + +/** + * @brief Check if it is OK to spin the motors + * + * @return true OK to spin motors + * @return false Not OK to spin motors + */ bool supervisorAreMotorsAllowedToRun(); +/** + * @brief Is the system ready to fly + * + * @return true + * @return false + */ bool supervisorCanFly(void); + +/** + * @brief Is the Crazyflie flying + * + * @return true + * @return false + */ bool supervisorIsFlying(void); + +/** + * @brief Is the Crazyflie tumbled + * + * @return true + * @return false + */ bool supervisorIsTumbled(void); From 1fd125915efaf3917fdb31d3e8b56e7db935c6a3 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 27 Apr 2023 16:27:15 +0200 Subject: [PATCH 22/30] Corrected isFlying --- src/modules/src/supervisor.c | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 247695238c..c80ff09b86 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -63,8 +63,8 @@ typedef struct { // The time (in ticks) of the first tumble event. 0=no tumble uint32_t initialTumbleTick; - // The time (in ticks) of the first low thrust event, when flying. 0=no low thrust event - uint32_t initialLowThrustTick; + // The time (in ticks) of the latest high thrust event. 0=no high thrust event yet + uint32_t latestThrustTick; supervisorState_t state; } SupervisorMem_t; @@ -90,8 +90,6 @@ bool supervisorIsTumbled() { // We say we are flying if one or more motors are running over the idle thrust. // static bool isFlyingCheck(SupervisorMem_t* this, const uint32_t tick) { - bool result = false; - bool isThrustOverIdle = false; const uint32_t idleThrust = powerDistributionGetIdleThrust(); for (int i = 0; i < NBR_OF_MOTORS; ++i) { @@ -103,13 +101,12 @@ static bool isFlyingCheck(SupervisorMem_t* this, const uint32_t tick) { } if (isThrustOverIdle) { - this->initialLowThrustTick = 0; - result = true; - } else { - if (0 == this->initialLowThrustTick) { - this->initialLowThrustTick = tick; - } - if ((tick - this->initialLowThrustTick) < IS_FLYING_HYSTERESIS_THRESHOLD) { + this->latestThrustTick = tick; + } + + bool result = false; + if (0 != this->latestThrustTick) { + if ((tick - this->latestThrustTick) < IS_FLYING_HYSTERESIS_THRESHOLD) { result = true; } } @@ -216,8 +213,8 @@ static supervisorConditionBits_t updateAndpopulateConditions(SupervisorMem_t* th static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t conditions) { this->canFly = supervisorAreMotorsAllowedToRun(); - this->isFlying = (bool)(conditions & SUPERVISOR_CB_IS_FLYING); - this->isTumbled = (bool)(conditions & SUPERVISOR_CB_IS_TUMBLED); + this->isFlying = (this->state == supervisorStateFlying) || (this->state == supervisorStateWarningLevelOut); + this->isTumbled = (conditions & SUPERVISOR_CB_IS_TUMBLED) != 0; } void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) { From 34b5375c39e89477aa066e92e8e41bfeb092c0f0 Mon Sep 17 00:00:00 2001 From: Tobias Antonson Date: Tue, 9 May 2023 17:23:28 +0200 Subject: [PATCH 23/30] Added canArm log variable and new not_passed transition --- src/modules/interface/supervisor.h | 15 +++++++++++++++ src/modules/src/supervisor.c | 14 ++++++++++++++ src/modules/src/supervisor_state_machine.c | 7 ++++++- 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index be64e44744..ad3e0339a4 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -59,6 +59,13 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint); */ bool supervisorAreMotorsAllowedToRun(); +/** + * @brief Check if system is allowed to arm + * + * @return true + * @return false + */ +bool supervisorCanSystemArm(); /** * @brief Is the system ready to fly * @@ -67,6 +74,14 @@ bool supervisorAreMotorsAllowedToRun(); */ bool supervisorCanFly(void); +/** + * @brief Is the system ready to be armed + * + * @return true + * @return false + */ +bool supervisorCanArm(void); + /** * @brief Is the Crazyflie flying * diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index c80ff09b86..9f3bd7e6a8 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -56,6 +56,7 @@ typedef struct { bool canFly; + bool canArm; bool isFlying; bool isTumbled; uint8_t paramEmergencyStop; @@ -78,6 +79,10 @@ bool supervisorCanFly() { return supervisorMem.canFly; } +bool supervisorCanArm() { + return supervisorMem.canArm; +} + bool supervisorIsFlying() { return supervisorMem.isFlying; } @@ -213,6 +218,7 @@ static supervisorConditionBits_t updateAndpopulateConditions(SupervisorMem_t* th static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t conditions) { this->canFly = supervisorAreMotorsAllowedToRun(); + this->canArm = supervisorCanSystemArm(); this->isFlying = (this->state == supervisorStateFlying) || (this->state == supervisorStateWarningLevelOut); this->isTumbled = (conditions & SUPERVISOR_CB_IS_TUMBLED) != 0; } @@ -270,6 +276,10 @@ bool supervisorAreMotorsAllowedToRun() { (this->state == supervisorStateWarningLevelOut); } +bool supervisorCanSystemArm() { + SupervisorMem_t* this = &supervisorMem; + return (this->state == supervisorStatePreFlChecksPassed); +} /** * System loggable variables to check different system states. */ @@ -278,6 +288,10 @@ LOG_GROUP_START(sys) * @brief Nonzero if system is ready to fly. */ LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorMem.canFly) +/** + * @brief Nonzero if system can be armed. + */ +LOG_ADD(LOG_UINT8, canArm, &supervisorMem.canArm) /** * @brief Nonzero if the system thinks it is flying */ diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 82f19ac509..6e2b350cfe 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -27,7 +27,7 @@ #include "supervisor_state_machine.h" #include "test_support.h" -// #define DEBUG_ME +#define DEBUG_ME #ifdef DEBUG_ME #define DEBUG_MODULE "SUPST" @@ -63,6 +63,11 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { .newState = supervisorStateReadyToFly, .mustBeSet = SUPERVISOR_CB_ARMED, .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP + }, + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, + .mustNotBeSet = SUPERVISOR_CB_NONE } }; From 381eea77d4eccf18d423a80e528daba14f14b7ae Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 May 2023 15:32:18 +0200 Subject: [PATCH 24/30] renamed test --- .../src/test_supervisor_state_machine.c | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c index 08a20af282..7154867569 100644 --- a/test/modules/src/test_supervisor_state_machine.c +++ b/test/modules/src/test_supervisor_state_machine.c @@ -32,7 +32,7 @@ void testTransitionWithNoConditions(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionOnePositiveConditionMet(void) { +void testTransitionOneRequiredConditionMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; @@ -43,7 +43,7 @@ void testTransitionOnePositiveConditionMet(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionOnePositiveConditionNotMet(void) { +void testTransitionOneRequiredConditionNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; @@ -54,7 +54,7 @@ void testTransitionOnePositiveConditionNotMet(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveConditionsMet(void) { +void testTransitionMultiRequiredConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; @@ -65,7 +65,7 @@ void testTransitionMultiPositiveConditionsMet(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveConditionsMetWithOtherPositives(void) { +void testTransitionMultiRequiredConditionsMetWithOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_EMERGENCY_STOP; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; @@ -76,7 +76,7 @@ void testTransitionMultiPositiveConditionsMetWithOtherPositives(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveConditionsOneMissing(void) { +void testTransitionMultiRequiredConditionsOneMissing(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; @@ -87,7 +87,7 @@ void testTransitionMultiPositiveConditionsOneMissing(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveConditionsOneMissingButOtherPositives(void) { +void testTransitionMultiRequiredConditionsOneMissingButOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; @@ -98,7 +98,7 @@ void testTransitionMultiPositiveConditionsOneMissingButOtherPositives(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionOneNegativeConditionMet(void) { +void testTransitionOneProhibitedConditionMet(void) { // Fixture supervisorConditionBits_t conditions = 0; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; @@ -109,7 +109,7 @@ void testTransitionOneNegativeConditionMet(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionOneNegativeConditionNotMet(void) { +void testTransitionOneProhibitedConditionNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; @@ -120,7 +120,7 @@ void testTransitionOneNegativeConditionNotMet(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionOneNegativeConditionNotMetWithOtherPositives(void) { +void testTransitionOneProhibitedConditionNotMetWithOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; @@ -131,7 +131,7 @@ void testTransitionOneNegativeConditionNotMetWithOtherPositives(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiNegativeConditionsMet(void) { +void testTransitionMultiProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = 0; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; @@ -142,7 +142,7 @@ void testTransitionMultiNegativeConditionsMet(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiNegativeConditionsOneNotMet(void) { +void testTransitionMultiProhibitedConditionsOneNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; @@ -153,7 +153,7 @@ void testTransitionMultiNegativeConditionsOneNotMet(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveAndNegativeConditionsMet(void) { +void testTransitionMultiRequiredAndProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; @@ -164,7 +164,7 @@ void testTransitionMultiPositiveAndNegativeConditionsMet(void) { assertStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveAndNegativeConditionsOnePositiveNotMet(void) { +void testTransitionMultiRequiredAndProhibitedConditionsOneRequiredNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; @@ -175,7 +175,7 @@ void testTransitionMultiPositiveAndNegativeConditionsOnePositiveNotMet(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveAndNegativeConditionsOneNegativeNotMet(void) { +void testTransitionMultiRequiredAndProhibitedConditionsOneProhibitedNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; @@ -186,7 +186,7 @@ void testTransitionMultiPositiveAndNegativeConditionsOneNegativeNotMet(void) { assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); } -void testTransitionMultiPositiveAndNegativeConditionsMultipleNotMet(void) { +void testTransitionMultiRequiredAndProhibitedConditionsMultipleNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; From 40e0442600545fb593084ed651e6a594ae4c3971 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 May 2023 15:34:04 +0200 Subject: [PATCH 25/30] Added more conditions back from preflight checks passed --- src/modules/src/supervisor_state_machine.c | 30 ++++++++++++++-------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 6e2b350cfe..27d178367c 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -60,15 +60,25 @@ static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { { - .newState = supervisorStateReadyToFly, - .mustBeSet = SUPERVISOR_CB_ARMED, - .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, + .mustNotBeSet = SUPERVISOR_CB_NONE }, { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, + .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, .mustNotBeSet = SUPERVISOR_CB_NONE - } + }, + { + .newState = supervisorStatePreFlChecksNotPassed, + .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, + .mustNotBeSet = SUPERVISOR_CB_NONE + }, + { + .newState = supervisorStateReadyToFly, + .mustBeSet = SUPERVISOR_CB_ARMED, + .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP + }, }; static SupervisorStateTransition_t transitionsReadyToFly[] = { @@ -209,13 +219,13 @@ TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t current for (int i = 0; i < transitions->length; i++) { const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; - const supervisorConditionBits_t maskPos = transitionDef->mustBeSet; - const supervisorConditionBits_t conditionsNotMetPos = (~conditions) & maskPos; + const supervisorConditionBits_t maskMustBeSet = transitionDef->mustBeSet; + const supervisorConditionBits_t conditionsNotMetMustBeSet = (~conditions) & maskMustBeSet; - const supervisorConditionBits_t maskNeg = transitionDef->mustNotBeSet; - const supervisorConditionBits_t conditionsNotMetNeg = conditions & maskNeg; + const supervisorConditionBits_t maskMustNotBeSet = transitionDef->mustNotBeSet; + const supervisorConditionBits_t conditionsNotMetMustNotBeSet = conditions & maskMustNotBeSet; - const supervisorConditionBits_t conditionsNotMet = conditionsNotMetPos | conditionsNotMetNeg; + const supervisorConditionBits_t conditionsNotMet = conditionsNotMetMustBeSet | conditionsNotMetMustNotBeSet; if (conditionsNotMet == 0) { newState = transitionDef->newState; From e9f82ae9cb901df2386b324aca2353bc98e27d23 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 May 2023 15:45:21 +0200 Subject: [PATCH 26/30] Removed arming again --- src/modules/interface/supervisor.h | 15 --------------- src/modules/src/supervisor.c | 14 -------------- src/modules/src/supervisor_state_machine.c | 2 +- 3 files changed, 1 insertion(+), 30 deletions(-) diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h index ad3e0339a4..be64e44744 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -59,13 +59,6 @@ void supervisorOverrideSetpoint(setpoint_t* setpoint); */ bool supervisorAreMotorsAllowedToRun(); -/** - * @brief Check if system is allowed to arm - * - * @return true - * @return false - */ -bool supervisorCanSystemArm(); /** * @brief Is the system ready to fly * @@ -74,14 +67,6 @@ bool supervisorCanSystemArm(); */ bool supervisorCanFly(void); -/** - * @brief Is the system ready to be armed - * - * @return true - * @return false - */ -bool supervisorCanArm(void); - /** * @brief Is the Crazyflie flying * diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 9f3bd7e6a8..c80ff09b86 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -56,7 +56,6 @@ typedef struct { bool canFly; - bool canArm; bool isFlying; bool isTumbled; uint8_t paramEmergencyStop; @@ -79,10 +78,6 @@ bool supervisorCanFly() { return supervisorMem.canFly; } -bool supervisorCanArm() { - return supervisorMem.canArm; -} - bool supervisorIsFlying() { return supervisorMem.isFlying; } @@ -218,7 +213,6 @@ static supervisorConditionBits_t updateAndpopulateConditions(SupervisorMem_t* th static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t conditions) { this->canFly = supervisorAreMotorsAllowedToRun(); - this->canArm = supervisorCanSystemArm(); this->isFlying = (this->state == supervisorStateFlying) || (this->state == supervisorStateWarningLevelOut); this->isTumbled = (conditions & SUPERVISOR_CB_IS_TUMBLED) != 0; } @@ -276,10 +270,6 @@ bool supervisorAreMotorsAllowedToRun() { (this->state == supervisorStateWarningLevelOut); } -bool supervisorCanSystemArm() { - SupervisorMem_t* this = &supervisorMem; - return (this->state == supervisorStatePreFlChecksPassed); -} /** * System loggable variables to check different system states. */ @@ -288,10 +278,6 @@ LOG_GROUP_START(sys) * @brief Nonzero if system is ready to fly. */ LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorMem.canFly) -/** - * @brief Nonzero if system can be armed. - */ -LOG_ADD(LOG_UINT8, canArm, &supervisorMem.canArm) /** * @brief Nonzero if the system thinks it is flying */ diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 27d178367c..338535933f 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -27,7 +27,7 @@ #include "supervisor_state_machine.h" #include "test_support.h" -#define DEBUG_ME +// #define DEBUG_ME #ifdef DEBUG_ME #define DEBUG_MODULE "SUPST" From 0f24391eb872d0fa01a49e5c5e68b69328b9e83b Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 May 2023 18:13:53 +0200 Subject: [PATCH 27/30] Started restructure state machine --- .../interface/supervisor_state_machine.h | 19 +- src/modules/src/supervisor_state_machine.c | 165 ++++++------ .../src/test_supervisor_state_machine.c | 254 +++++++++++++----- 3 files changed, 298 insertions(+), 140 deletions(-) diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h index db653a56ca..341ca9228a 100644 --- a/src/modules/interface/supervisor_state_machine.h +++ b/src/modules/interface/supervisor_state_machine.h @@ -63,10 +63,25 @@ typedef uint32_t supervisorConditionBits_t; #define SUPERVISOR_CB_EMERGENCY_STOP (1 << supervisorConditionEmergencyStop) +// Enum that is used to describe how to combine the bits in the required field +typedef enum { + supervisorAll = 0, // AKA and + supervisorAny, // AKA or + supervisorAlways, + supervisorNever, +} SupervisorConditionCombiner_t; + +// Describes the requirements for a state transition typedef struct { supervisorState_t newState; - supervisorConditionBits_t mustBeSet; - supervisorConditionBits_t mustNotBeSet; + + supervisorConditionBits_t triggers; + supervisorConditionBits_t negatedTriggers; + SupervisorConditionCombiner_t triggerCombiner; + + supervisorConditionBits_t blockers; + supervisorConditionBits_t negatedBlockers; + SupervisorConditionCombiner_t blockerCombiner; } SupervisorStateTransition_t; typedef struct { diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 338535933f..3e8be00eb2 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -53,142 +53,157 @@ static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOf static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { { .newState = supervisorStatePreFlChecksPassed, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP + + .triggers = SUPERVISOR_CB_NONE, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAlways, + + .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .blockerCombiner = supervisorAny, } }; static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStateReadyToFly, - .mustBeSet = SUPERVISOR_CB_ARMED, - .mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP + + .triggers = SUPERVISOR_CB_ARMED, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedBlockers = SUPERVISOR_CB_NONE, + .blockerCombiner = supervisorAny, }, }; static SupervisorStateTransition_t transitionsReadyToFly[] = { { .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_ARMED - }, - { - .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED, + .negatedTriggers = SUPERVISOR_CB_ARMED, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStateFlying, - .mustBeSet = SUPERVISOR_CB_IS_FLYING, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_IS_FLYING, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, } }; static SupervisorStateTransition_t transitionsFlying[] = { { .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_EMERGENCY_STOP, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_ARMED + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_ARMED, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStateWarningLevelOut, - .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_WARNING, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStateLanded, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_IS_FLYING + + .triggerCombiner = supervisorAlways, + + .blockers = SUPERVISOR_CB_IS_FLYING, + .negatedBlockers = SUPERVISOR_CB_NONE, + .blockerCombiner = supervisorAny, } }; static SupervisorStateTransition_t transitionsLanded[] = { { .newState = supervisorStateReset, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, }, }; static SupervisorStateTransition_t transitionsReset[] = { { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, }, }; static SupervisorStateTransition_t transitionsWarningLevelOut[] = { { .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, - .mustNotBeSet = SUPERVISOR_CB_NONE - }, - { - .newState = supervisorStateExceptFreeFall, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStateFlying, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_COMMANDER_WDT_WARNING | SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT + + .triggers = SUPERVISOR_CB_NONE, + .negatedTriggers = SUPERVISOR_CB_COMMANDER_WDT_WARNING | SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, }, }; static SupervisorStateTransition_t transitionsExceptFreeFall[] = { { .newState = supervisorStateLocked, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, }, }; static SupervisorStateTransition_t transitionsLocked[] = { { .newState = supervisorStateLocked, - .mustBeSet = SUPERVISOR_CB_NONE, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggerCombiner = supervisorNever, + + .blockerCombiner = supervisorAlways, }, }; @@ -219,13 +234,13 @@ TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t current for (int i = 0; i < transitions->length; i++) { const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; - const supervisorConditionBits_t maskMustBeSet = transitionDef->mustBeSet; - const supervisorConditionBits_t conditionsNotMetMustBeSet = (~conditions) & maskMustBeSet; + const supervisorConditionBits_t maskRequired = transitionDef->triggers; + const supervisorConditionBits_t conditionsNotMetRequired = (~conditions) & maskRequired; - const supervisorConditionBits_t maskMustNotBeSet = transitionDef->mustNotBeSet; - const supervisorConditionBits_t conditionsNotMetMustNotBeSet = conditions & maskMustNotBeSet; + const supervisorConditionBits_t maskBlocking = transitionDef->blockers; + const supervisorConditionBits_t conditionsNotMetBlocking = conditions & maskBlocking; - const supervisorConditionBits_t conditionsNotMet = conditionsNotMetMustBeSet | conditionsNotMetMustNotBeSet; + const supervisorConditionBits_t conditionsNotMet = conditionsNotMetRequired | conditionsNotMetBlocking; if (conditionsNotMet == 0) { newState = transitionDef->newState; diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c index 7154867569..f7e6eb641a 100644 --- a/test/modules/src/test_supervisor_state_machine.c +++ b/test/modules/src/test_supervisor_state_machine.c @@ -10,8 +10,13 @@ supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBits_t triggerBitField, const SupervisorStateTransitionList_t* transitions); // Helpers -static void assertStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet); -static void assertNoStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet); +static void assertStateTransition(supervisorConditionBits_t conditions, + supervisorConditionBits_t triggers, supervisorConditionBits_t negatedTriggers, SupervisorConditionCombiner_t triggerCombiner, + supervisorConditionBits_t blockers, supervisorConditionBits_t negatedBlockers, SupervisorConditionCombiner_t blockerCombiner); + +static void assertNoStateTransition(supervisorConditionBits_t conditions, + supervisorConditionBits_t triggers, supervisorConditionBits_t negatedTriggers, SupervisorConditionCombiner_t triggerCombiner, + supervisorConditionBits_t blockers, supervisorConditionBits_t negatedBlockers, SupervisorConditionCombiner_t blockerCombiner); void setUp(void) { // Empty @@ -24,177 +29,273 @@ void tearDown(void) { void testTransitionWithNoConditions(void) { // Fixture supervisorConditionBits_t conditions = 123; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionOneRequiredConditionMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionOneRequiredConditionNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredConditionsMetWithOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_EMERGENCY_STOP; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredConditionsOneMissing(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredConditionsOneMissingButOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_NONE; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionOneProhibitedConditionMet(void) { // Fixture supervisorConditionBits_t conditions = 0; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionOneProhibitedConditionNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionOneProhibitedConditionNotMetWithOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = 0; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiProhibitedConditionsOneNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_NONE; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredAndProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertStateTransition(conditions, mustBeSet, mustNotBeSet); + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredAndProhibitedConditionsOneRequiredNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredAndProhibitedConditionsOneProhibitedNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testTransitionMultiRequiredAndProhibitedConditionsMultipleNotMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; - supervisorConditionBits_t mustBeSet = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; - supervisorConditionBits_t mustNotBeSet = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; // Test // Assert - assertNoStateTransition(conditions, mustBeSet, mustNotBeSet); + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } void testFirstValidTransitionIsChosen(void) { @@ -207,18 +308,32 @@ void testFirstValidTransitionIsChosen(void) { SupervisorStateTransition_t transitions[] = { { .newState = supervisorStateLanded, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_EMERGENCY_STOP + + .triggers = SUPERVISOR_CB_IS_TUMBLED, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockers = SUPERVISOR_CB_EMERGENCY_STOP, + .negatedBlockers = SUPERVISOR_CB_NONE, + .blockerCombiner = supervisorAny, }, { // We expect this state to be chosen .newState = expected, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_IS_TUMBLED, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, }, { .newState = supervisorStatePreFlChecksNotPassed, - .mustBeSet = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, - .mustNotBeSet = SUPERVISOR_CB_NONE + + .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, } }; @@ -233,15 +348,24 @@ void testFirstValidTransitionIsChosen(void) { // Helpers //////////////////////////////////////////////// -static bool check_state_transition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { +static bool check_state_transition(supervisorConditionBits_t conditions, + supervisorConditionBits_t triggers, supervisorConditionBits_t negatedTriggers, SupervisorConditionCombiner_t triggerCombiner, + supervisorConditionBits_t blockers, supervisorConditionBits_t negatedBlockers, SupervisorConditionCombiner_t blockerCombiner) { + // Fixture const supervisorState_t currentState = supervisorStateFlying; SupervisorStateTransition_t transitions[] = { { .newState = supervisorStateLanded, - .mustBeSet = mustBeSet, - .mustNotBeSet = mustNotBeSet + + .triggers = triggers, + .negatedTriggers = negatedTriggers, + .triggerCombiner = triggerCombiner, + + .blockers = blockers, + .negatedBlockers = negatedBlockers, + .blockerCombiner = blockerCombiner, } }; @@ -254,10 +378,14 @@ static bool check_state_transition(supervisorConditionBits_t conditions, supervi return newState != currentState; } -static void assertStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { - TEST_ASSERT_TRUE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) +static void assertStateTransition(supervisorConditionBits_t conditions, + supervisorConditionBits_t triggers, supervisorConditionBits_t negatedTriggers, SupervisorConditionCombiner_t triggerCombiner, + supervisorConditionBits_t blockers, supervisorConditionBits_t negatedBlockers, SupervisorConditionCombiner_t blockerCombiner) { + TEST_ASSERT_TRUE(check_state_transition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner)); } -static void assertNoStateTransition(supervisorConditionBits_t conditions, supervisorConditionBits_t mustBeSet, supervisorConditionBits_t mustNotBeSet) { - TEST_ASSERT_FALSE(check_state_transition(conditions, mustBeSet, mustNotBeSet)) +static void assertNoStateTransition(supervisorConditionBits_t conditions, + supervisorConditionBits_t triggers, supervisorConditionBits_t negatedTriggers, SupervisorConditionCombiner_t triggerCombiner, + supervisorConditionBits_t blockers, supervisorConditionBits_t negatedBlockers, SupervisorConditionCombiner_t blockerCombiner) { + TEST_ASSERT_FALSE(check_state_transition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner)); } From f4d0ad20a06b843f8e4c8227b672895242854570 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 11 May 2023 20:00:22 +0200 Subject: [PATCH 28/30] "All" and "any" for triggers and blockers --- src/modules/src/supervisor_state_machine.c | 44 ++++++++-- .../src/test_supervisor_state_machine.c | 87 ++++++++++++++++++- 2 files changed, 121 insertions(+), 10 deletions(-) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 3e8be00eb2..dfd2417a2d 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -54,8 +54,6 @@ static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { { .newState = supervisorStatePreFlChecksPassed, - .triggers = SUPERVISOR_CB_NONE, - .negatedTriggers = SUPERVISOR_CB_NONE, .triggerCombiner = supervisorAlways, .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, @@ -229,20 +227,48 @@ bool supervisorStateMachineInit() { return true; } +static bool areAllSet(const supervisorConditionBits_t conditions, const supervisorConditionBits_t requirements) { + return (~conditions & requirements) == 0; +} + +static bool isAnySet(const supervisorConditionBits_t conditions, const supervisorConditionBits_t requirements) { + return (conditions & requirements) != 0; +} + +static bool areConditionsMet(const supervisorConditionBits_t conditions, const supervisorConditionBits_t requirements, const SupervisorConditionCombiner_t combiner) { + bool result = false; + + switch(combiner) { + case supervisorAll: + result = areAllSet(conditions, requirements); + break; + case supervisorAny: + result = isAnySet(conditions, requirements); + break; + case supervisorAlways: + result = true; + break; + case supervisorNever: + result = false; + break; + default: + break; + } + + return result; +} + TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t currentState, const supervisorConditionBits_t conditions, const SupervisorStateTransitionList_t* transitions) { supervisorState_t newState = currentState; for (int i = 0; i < transitions->length; i++) { const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; - const supervisorConditionBits_t maskRequired = transitionDef->triggers; - const supervisorConditionBits_t conditionsNotMetRequired = (~conditions) & maskRequired; - - const supervisorConditionBits_t maskBlocking = transitionDef->blockers; - const supervisorConditionBits_t conditionsNotMetBlocking = conditions & maskBlocking; + const bool triggerConditionsMet = areConditionsMet(conditions, transitionDef->triggers, transitionDef->triggerCombiner); + const bool blockerConditionsMet = areConditionsMet(conditions, transitionDef->blockers, transitionDef->blockerCombiner); - const supervisorConditionBits_t conditionsNotMet = conditionsNotMetRequired | conditionsNotMetBlocking; + const bool conditionsMet = triggerConditionsMet && !blockerConditionsMet; - if (conditionsNotMet == 0) { + if (conditionsMet) { newState = transitionDef->newState; break; } diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c index f7e6eb641a..a71055af94 100644 --- a/test/modules/src/test_supervisor_state_machine.c +++ b/test/modules/src/test_supervisor_state_machine.c @@ -26,7 +26,7 @@ void tearDown(void) { // Empty } -void testTransitionWithNoConditions(void) { +void testTransitionWithNoConditionsTriggerAlways(void) { // Fixture supervisorConditionBits_t conditions = 123; @@ -43,6 +43,40 @@ void testTransitionWithNoConditions(void) { assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionWithNoConditionsTriggerNever(void) { + // Fixture + supervisorConditionBits_t conditions = 123; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorNever; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionWithNoConditionsBlockAlways(void) { + // Fixture + supervisorConditionBits_t conditions = 123; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAlways; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionOneRequiredConditionMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; @@ -128,6 +162,23 @@ void testTransitionMultiRequiredConditionsOneMissing(void) { assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionMultiRequiredConditionsOneMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAny; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionMultiRequiredConditionsOneMissingButOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; @@ -230,6 +281,40 @@ void testTransitionMultiProhibitedConditionsOneNotMet(void) { assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionMultiProhibitedConditionsAllNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAll; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiProhibitedConditionsAllMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAll; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionMultiRequiredAndProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; From 5f35cff14c2786020449de0ca7ded281c6efa663 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 15 May 2023 14:33:31 +0200 Subject: [PATCH 29/30] Added negated triggers and blockers --- .../functional-areas/supervisor/conditions.md | 2 + docs/functional-areas/supervisor/index.md | 4 +- .../supervisor/transitions.md | 30 +++ src/modules/src/supervisor_state_machine.c | 15 +- .../src/test_supervisor_state_machine.c | 238 ++++++++++++++++++ 5 files changed, 279 insertions(+), 10 deletions(-) create mode 100644 docs/functional-areas/supervisor/transitions.md diff --git a/docs/functional-areas/supervisor/conditions.md b/docs/functional-areas/supervisor/conditions.md index 1b6dc0509d..c55c8376c7 100644 --- a/docs/functional-areas/supervisor/conditions.md +++ b/docs/functional-areas/supervisor/conditions.md @@ -10,3 +10,5 @@ All conditions are collected in a bit field that expresses the full state of the that are relevant to the supervisor. All conditions can be found in the `src/modules/interface/supervisor_state_machine.h` file. + +The condition bit field is used to trigger [state transitions](transitions.md). diff --git a/docs/functional-areas/supervisor/index.md b/docs/functional-areas/supervisor/index.md index 58197f900a..a26d45fb37 100644 --- a/docs/functional-areas/supervisor/index.md +++ b/docs/functional-areas/supervisor/index.md @@ -14,8 +14,8 @@ situations or for protection. The update sequence is separated into a few steps: 1. Collect data from sensors and the system. We call these [conditions](conditions.md). -2. Based on the conditions, check if the state machine should transition into a new [state](states.md) -3. If there is a state transition, possibly execute one ore more actions +2. Based on the conditions, check if the state machine should [transition](transitions.md) into a new [state](states.md) +3. If there is a state [transition](transitions.md), possibly execute one ore more actions 4. Set the new state ## Modifying behavior diff --git a/docs/functional-areas/supervisor/transitions.md b/docs/functional-areas/supervisor/transitions.md new file mode 100644 index 0000000000..0ecebed10e --- /dev/null +++ b/docs/functional-areas/supervisor/transitions.md @@ -0,0 +1,30 @@ +--- +title: Supervisor transitions +page_id: supervisor_transitions +--- + +A state transition takes the state machine from one state to another. The [conditions bit field](conditions.md) is used to identify +which, if any transition to trigger. Transitions are defined as a list per state and are evaluated in order. If a +transition is triggered, the evaluation is terminated and no further transitions will be checked. That means +that the order of a transition list sets the priority, two transitions might have settings that makes both valid, but +the first one will "win". + +A state transition is defined with a bit field of triggers. The bits in the trigger bit field are compared to the bits +in the current [conditions bit field](conditions.md) to see if they are set. There is also a bit field with negated triggers where the +bits in the condition bit field should be zero to trigger. To tie it together there is a trigger combiner that describes +the logical operation to use to decide whether the state transition should be triggered or not +* `supervisorAll` = all `trigger` bits must be set and all `negated trigger` bits must be zero in the condition bit field. +* `supervisorAny` = at least one `trigger` bit must be set or at least one `negated trigger` bits must be zero +* `supervisorAlways` = ignore the condition bit field and treat the condition as true +* `supervisorNever` = ignore the condition bit field and treat the condition as false + +The second part of a state transition definition is a blocking functionality that works the same way as a trigger, but +is negated. It contains a bit field called blockers, a second bit field called negated blocker and a blocker combiner. +If the blocker evaluates to true, the trigger will not happen. + +The final data of a state transition definition is the new state that the state machine will enter, if the trigger +condition is met and the blocking condition is not met. + +The system of state transition definitions with triggers, negated triggers, blockers and negated blockers, match with +combiners provides a high degree of freedom. Note that it is possible to express a set of state transitions with trigger +conditions in multiple ways. diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index dfd2417a2d..fb20ed4c61 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -235,15 +235,15 @@ static bool isAnySet(const supervisorConditionBits_t conditions, const superviso return (conditions & requirements) != 0; } -static bool areConditionsMet(const supervisorConditionBits_t conditions, const supervisorConditionBits_t requirements, const SupervisorConditionCombiner_t combiner) { +static bool areConditionsMet(const supervisorConditionBits_t conditions, const supervisorConditionBits_t requirements, const supervisorConditionBits_t negRequirements, const SupervisorConditionCombiner_t combiner) { bool result = false; switch(combiner) { case supervisorAll: - result = areAllSet(conditions, requirements); + result = areAllSet(conditions, requirements) && !isAnySet(conditions, negRequirements); break; case supervisorAny: - result = isAnySet(conditions, requirements); + result = isAnySet(conditions, requirements) || !areAllSet(conditions, negRequirements); break; case supervisorAlways: result = true; @@ -263,12 +263,11 @@ TESTABLE_STATIC supervisorState_t findTransition(const supervisorState_t current for (int i = 0; i < transitions->length; i++) { const SupervisorStateTransition_t* transitionDef = &transitions->transitionList[i]; - const bool triggerConditionsMet = areConditionsMet(conditions, transitionDef->triggers, transitionDef->triggerCombiner); - const bool blockerConditionsMet = areConditionsMet(conditions, transitionDef->blockers, transitionDef->blockerCombiner); + const bool isTriggerMatch = areConditionsMet(conditions, transitionDef->triggers, transitionDef->negatedTriggers, transitionDef->triggerCombiner); + const bool isBlockerMatch = areConditionsMet(conditions, transitionDef->blockers, transitionDef->negatedBlockers, transitionDef->blockerCombiner); - const bool conditionsMet = triggerConditionsMet && !blockerConditionsMet; - - if (conditionsMet) { + const bool isStateTransitionValid = isTriggerMatch && !isBlockerMatch; + if (isStateTransitionValid) { newState = transitionDef->newState; break; } diff --git a/test/modules/src/test_supervisor_state_machine.c b/test/modules/src/test_supervisor_state_machine.c index a71055af94..7d46d51562 100644 --- a/test/modules/src/test_supervisor_state_machine.c +++ b/test/modules/src/test_supervisor_state_machine.c @@ -111,6 +111,40 @@ void testTransitionOneRequiredConditionNotMet(void) { assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionOneNegatedRequiredConditionNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionOneNegatedRequiredConditionMet(void) { + // Fixture + supervisorConditionBits_t conditions = 0; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionMultiRequiredConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; @@ -179,6 +213,142 @@ void testTransitionMultiRequiredConditionsOneMet(void) { assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionMultiNegatedRequiredConditionOneNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiNegatedRequiredConditionsMet(void) { + // Fixture + supervisorConditionBits_t conditions = 0; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedRequiredConditionsAllMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedRequiredConditionsOnePositiveNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedRequiredConditionsOneNegativeNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAll; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedOnePositiveRequirementMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED; + SupervisorConditionCombiner_t triggerCombiner = supervisorAny; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedNoRequirementMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_COMMANDER_WDT_WARNING; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_COMMANDER_WDT_WARNING; + SupervisorConditionCombiner_t triggerCombiner = supervisorAny; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiMixedOneNegativeRequirementMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_COMMANDER_WDT_WARNING; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_ARMED | SUPERVISOR_CB_COMMANDER_WDT_WARNING; + SupervisorConditionCombiner_t triggerCombiner = supervisorAny; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorNever; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionMultiRequiredConditionsOneMissingButOtherBitsSet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; @@ -315,6 +485,74 @@ void testTransitionMultiProhibitedConditionsAllMet(void) { assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); } +void testTransitionMultiNegativeProhibitedConditionsNoneMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiNegativeProhibitedConditionsOneMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiNegativeProhibitedConditionsOneNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + SupervisorConditionCombiner_t blockerCombiner = supervisorAll; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiNegativeProhibitedConditionsAllMet(void) { + // Fixture + supervisorConditionBits_t conditions = 0; + + supervisorConditionBits_t triggers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedTriggers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t triggerCombiner = supervisorAlways; + + supervisorConditionBits_t blockers = SUPERVISOR_CB_NONE; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + SupervisorConditionCombiner_t blockerCombiner = supervisorAll; + + // Test + // Assert + assertNoStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + void testTransitionMultiRequiredAndProhibitedConditionsMet(void) { // Fixture supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED | SUPERVISOR_CB_IS_TUMBLED; From 183fe51567a90ba4aaefb74383041611ae3a6afd Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 15 May 2023 14:34:39 +0200 Subject: [PATCH 30/30] Updated transitions --- src/modules/src/supervisor_state_machine.c | 24 ++++++++-------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index fb20ed4c61..d1a3cf9bb3 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/src/modules/src/supervisor_state_machine.c @@ -57,6 +57,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { .triggerCombiner = supervisorAlways, .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedBlockers = SUPERVISOR_CB_NONE, .blockerCombiner = supervisorAny, } }; @@ -85,19 +86,10 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { }; static SupervisorStateTransition_t transitionsReadyToFly[] = { - { - .newState = supervisorStateExceptFreeFall, - - .triggers = SUPERVISOR_CB_EMERGENCY_STOP, - .negatedTriggers = SUPERVISOR_CB_NONE, - .triggerCombiner = supervisorAll, - - .blockerCombiner = supervisorNever, - }, { .newState = supervisorStatePreFlChecksNotPassed, - .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED, + .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP, .negatedTriggers = SUPERVISOR_CB_ARMED, .triggerCombiner = supervisorAny, @@ -136,11 +128,11 @@ static SupervisorStateTransition_t transitionsFlying[] = { { .newState = supervisorStateLanded, - .triggerCombiner = supervisorAlways, + .triggers = SUPERVISOR_CB_NONE, + .negatedTriggers = SUPERVISOR_CB_IS_FLYING, + .triggerCombiner = supervisorAll, - .blockers = SUPERVISOR_CB_IS_FLYING, - .negatedBlockers = SUPERVISOR_CB_NONE, - .blockerCombiner = supervisorAny, + .blockerCombiner = supervisorNever, } }; @@ -168,8 +160,8 @@ static SupervisorStateTransition_t transitionsWarningLevelOut[] = { { .newState = supervisorStateExceptFreeFall, - .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED, - .negatedTriggers = SUPERVISOR_CB_NONE, + .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_ARMED, .triggerCombiner = supervisorAny, .blockerCombiner = supervisorNever,