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/docs/functional-areas/supervisor/conditions.md b/docs/functional-areas/supervisor/conditions.md new file mode 100644 index 0000000000..c55c8376c7 --- /dev/null +++ b/docs/functional-areas/supervisor/conditions.md @@ -0,0 +1,14 @@ +--- +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. + +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 new file mode 100644 index 0000000000..a26d45fb37 --- /dev/null +++ b/docs/functional-areas/supervisor/index.md @@ -0,0 +1,35 @@ +--- +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. + +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](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 + +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 new file mode 100644 index 0000000000..9b22c01b51 --- /dev/null +++ b/docs/functional-areas/supervisor/states.md @@ -0,0 +1,62 @@ +--- +title: Supervisor states +page_id: supervisor_states +--- + +## State diagram +{% ditaa --alt "Supervisor states" %} + Boot| + V ++-------------------+ ++ Pre-flight checks +<------------+ ++ not passed +<--+ | ++-------+-----------+ | | + | ^ | | + V | | | ++---------+---------+ | | ++ Pre-flight checks + | | ++ passed + | | ++-------+-----------+ | | + | ^ | +-------+---------+ + Arming| | | + Reset + + V | | + + ++---------+---------+ | +-----------------+ ++ Ready to fly +---+ ^ ++ + | ++--------+----------+ | + | | + V | ++-------------------+ | +-------------------+ ++ Flying +------------|-------->+ Warning + ++ +<-----------|---------+ Level out + ++--------+-------+--+ | +---------+---------+ + | | | | + V +---------------|-----+ V ++-------------------+ | | +-------------------+ ++ Landed +------------+ +-->+ Exception + ++ | + Free fall + ++--------+----------+ +---------+---------+ + | + | + +--------------+ + | + V + +-------------------+ + + Lock + + + + + +-------------------+ +{% 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/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/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/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/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/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/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/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/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 ad9b30fc9d..be64e44744 100644 --- a/src/modules/interface/supervisor.h +++ b/src/modules/interface/supervisor.h @@ -28,8 +28,57 @@ #include "stabilizer_types.h" -void supervisorUpdate(const sensorData_t *data); +/** + * @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); diff --git a/src/modules/interface/supervisor_state_machine.h b/src/modules/interface/supervisor_state_machine.h new file mode 100644 index 0000000000..341ca9228a --- /dev/null +++ b/src/modules/interface/supervisor_state_machine.h @@ -0,0 +1,95 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( 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, + supervisorStateLocked, + supervisorState_NrOfStates, +} supervisorState_t; + +// Conditions supported by the supervisor +enum { + supervisorConditionArmed = 0, + supervisorConditionChargerConnected, + supervisorConditionIsFlying, + supervisorConditionIsTumbled, + supervisorConditionCommanderWdtWarning, + supervisorConditionCommanderWdtTimeout, + supervisorConditionEmergencyStop, +}; + +typedef uint32_t supervisorConditionBits_t; + +// Condition bit definitions +#define SUPERVISOR_CB_NONE (0) +#define SUPERVISOR_CB_ARMED (1 << supervisorConditionArmed) +#define SUPERVISOR_CB_CHARGER_CONNECTED (1 << supervisorConditionChargerConnected) +#define SUPERVISOR_CB_IS_FLYING (1 << supervisorConditionIsFlying) +#define SUPERVISOR_CB_IS_TUMBLED (1 << supervisorConditionIsTumbled) +#define SUPERVISOR_CB_COMMANDER_WDT_WARNING (1 << supervisorConditionCommanderWdtWarning) +#define SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT (1 << supervisorConditionCommanderWdtTimeout) +#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 triggers; + supervisorConditionBits_t negatedTriggers; + SupervisorConditionCombiner_t triggerCombiner; + + supervisorConditionBits_t blockers; + supervisorConditionBits_t negatedBlockers; + SupervisorConditionCombiner_t blockerCombiner; +} 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 supervisorConditionBits_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/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/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/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/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/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/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/stabilizer.c b/src/modules/src/stabilizer.c index c41aa4673e..09d8aff3fb 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(); @@ -233,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). @@ -256,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(); @@ -270,48 +276,45 @@ 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(); + + 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); } - commanderGetSetpoint(&setpoint, &state); - compressSetpoint(); - collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep); + // Critical for safety, be careful if you modify this code! + // Let the supervisor update it's view of the current situation + supervisorUpdate(&sensorData, &setpoint, stabilizerStep); - controller(&control, &setpoint, &sensorData, &state, stabilizerStep); + // Let the collision avoidance module modify the setpoint, if needed + collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, stabilizerStep); - checkEmergencyStopTimeout(); + // Critical for safety, be careful if you modify this code! + // Let the supervisor modify the setpoint to handle exceptional conditions + supervisorOverrideSetpoint(&setpoint); - // - // 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); + controller(&control, &setpoint, &sensorData, &state, stabilizerStep); - if (emergencyStop || (systemIsArmed() == false)) { - motorsStop(); + // 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 (areMotorsAllowedToRun) { + controlMotors(&control); } else { - powerDistribution(&control, &motorThrustUncapped); - batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped); - powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm); - setMotorRatios(&motorPwm); + motorsStop(); } + // Compute compressed log formats + compressState(); + compressSetpoint(); + #ifdef CONFIG_DECK_USD // Log data to uSD card if configured if (usddeckLoggingEnabled() @@ -337,25 +340,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 +355,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..c80ff09b86 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 @@ -29,62 +29,89 @@ #include #include "log.h" +#include "param.h" #include "motors.h" #include "power_distribution.h" #include "pm.h" -#include "stabilizer.h" #include "supervisor.h" +#include "supervisor_state_machine.h" #include "platform_defaults.h" +#include "crtp_localization_service.h" +#include "system.h" -/* Minimum summed motor PWM that means we are flying */ -#define SUPERVISOR_FLIGHT_THRESHOLD 1000 +#define DEBUG_MODULE "SUP" +#include "debug.h" -/* Number of times in a row we need to see a condition before acting upon it */ -#define SUPERVISOR_HYSTERESIS_THRESHOLD 30 -static bool canFly; -static bool isFlying; -static bool isTumbled; +#define DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT (M2T(1000)) -bool supervisorCanFly() -{ - return canFly; -} +// 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) + +typedef struct { + bool canFly; + bool isFlying; + bool isTumbled; + uint8_t paramEmergencyStop; + + // The time (in ticks) of the first tumble event. 0=no tumble + uint32_t initialTumbleTick; + + // The time (in ticks) of the latest high thrust event. 0=no high thrust event yet + uint32_t latestThrustTick; + + supervisorState_t state; +} SupervisorMem_t; + +static SupervisorMem_t supervisorMem; + +const static setpoint_t nullSetpoint; -bool supervisorIsFlying() -{ - return isFlying; + +bool supervisorCanFly() { + return supervisorMem.canFly; } -bool supervisorIsTumbled() -{ - return isTumbled; +bool supervisorIsFlying() { + return supervisorMem.isFlying; } -// -// 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(); +bool supervisorIsTumbled() { + return supervisorMem.isTumbled; } // -// 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 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->latestThrustTick = tick; } - return sumRatio > SUPERVISOR_FLIGHT_THRESHOLD; + bool result = false; + if (0 != this->latestThrustTick) { + if ((tick - this->latestThrustTick) < IS_FLYING_HYSTERESIS_THRESHOLD) { + result = true; + } + } + + return result; } // @@ -94,37 +121,153 @@ 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(SupervisorMem_t* this, const sensorData_t *data, const uint32_t tick) { 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) { + if (0 == this->initialTumbleTick) { + this->initialTumbleTick = tick; + } + if ((tick - this->initialTumbleTick) > TUMBLE_HYSTERESIS_THRESHOLD) { return true; } } else { - hysteresis = 0; + this->initialTumbleTick = 0; } return false; } -void supervisorUpdate(const sensorData_t *data) -{ - isFlying = isFlyingCheck(); +static bool checkEmergencyStopWatchdog(const uint32_t tick) { + bool isOk = true; + + const uint32_t latestNotification = locSrvGetEmergencyStopWatchdogNotificationTick(); + if (latestNotification > 0) { + isOk = tick < (latestNotification + DEFAULT_EMERGENCY_STOP_WATCHDOG_TIMEOUT); + } + + 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"); + } +} + +static supervisorConditionBits_t updateAndpopulateConditions(SupervisorMem_t* this, const sensorData_t *sensors, const setpoint_t* setpoint, const uint32_t currentTick) { + supervisorConditionBits_t conditions = 0; + + if (systemIsArmed()) { + conditions |= SUPERVISOR_CB_ARMED; + } + + if (pmIsChargerConnected()) { + conditions |= SUPERVISOR_CB_CHARGER_CONNECTED; + } + + const bool isFlying = isFlyingCheck(this, currentTick); + if (isFlying) { + conditions |= SUPERVISOR_CB_IS_FLYING; + } + + 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; + } + 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 = (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) { + if (!RATE_DO_EXECUTE(RATE_SUPERVISOR, stabilizerStep)) { + return; + } + + SupervisorMem_t* this = &supervisorMem; + const uint32_t currentTick = xTaskGetTickCount(); - isTumbled = isTumbledCheck(data); - #if SUPERVISOR_TUMBLE_CHECK_ENABLE - if (isTumbled && isFlying) { - stabilizerSetEmergencyStop(); + 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; } - #endif - canFly = canFlyCheck(); + + updateLogData(this, conditions); +} + +void supervisorOverrideSetpoint(setpoint_t* setpoint) { + SupervisorMem_t* this = &supervisorMem; + switch(this->state){ + case supervisorStateReadyToFly: + // Fall through + case supervisorStateFlying: + // Do nothing + break; + + case supervisorStateWarningLevelOut: + 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; + + default: + // Replace with null setpoint to stop motors + memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint)); + break; + } +} + +bool supervisorAreMotorsAllowedToRun() { + SupervisorMem_t* this = &supervisorMem; + return (this->state == supervisorStateReadyToFly) || + (this->state == supervisorStateFlying) || + (this->state == supervisorStateWarningLevelOut); } /** @@ -132,15 +275,23 @@ void supervisorUpdate(const sensorData_t *data) */ 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, &canFly) +LOG_ADD_CORE(LOG_UINT8, canfly, &supervisorMem.canFly) /** * @brief Nonzero if the system thinks it is flying */ -LOG_ADD_CORE(LOG_UINT8, isFlying, &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, &isTumbled) +LOG_ADD_CORE(LOG_UINT8, isTumbled, &supervisorMem.isTumbled) LOG_GROUP_STOP(sys) + + +PARAM_GROUP_START(stabilizer) +/** + * @brief If set to nonzero will turn off power + */ +PARAM_ADD_CORE(PARAM_UINT8, stop, &supervisorMem.paramEmergencyStop) +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..d1a3cf9bb3 --- /dev/null +++ b/src/modules/src/supervisor_state_machine.c @@ -0,0 +1,288 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( 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", + "Locked", +}; +static_assert(sizeof(stateNames) / sizeof(stateNames[0]) == supervisorState_NrOfStates); +#endif + + +// State transition definitions +static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = { + { + .newState = supervisorStatePreFlChecksPassed, + + .triggerCombiner = supervisorAlways, + + .blockers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedBlockers = SUPERVISOR_CB_NONE, + .blockerCombiner = supervisorAny, + } +}; + +static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = { + { + .newState = supervisorStatePreFlChecksNotPassed, + + .triggers = SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStateReadyToFly, + + .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 = supervisorStatePreFlChecksNotPassed, + + .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_CHARGER_CONNECTED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_ARMED, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStateFlying, + + .triggers = SUPERVISOR_CB_IS_FLYING, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, + } +}; + +static SupervisorStateTransition_t transitionsFlying[] = { + { + .newState = supervisorStateExceptFreeFall, + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_ARMED, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStateWarningLevelOut, + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_WARNING, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStateLanded, + + .triggers = SUPERVISOR_CB_NONE, + .negatedTriggers = SUPERVISOR_CB_IS_FLYING, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, + } +}; + +static SupervisorStateTransition_t transitionsLanded[] = { + { + .newState = supervisorStateReset, + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, + }, +}; + +static SupervisorStateTransition_t transitionsReset[] = { + { + .newState = supervisorStatePreFlChecksNotPassed, + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, + }, +}; + +static SupervisorStateTransition_t transitionsWarningLevelOut[] = { + { + .newState = supervisorStateExceptFreeFall, + + .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_ARMED, + .triggerCombiner = supervisorAny, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStateFlying, + + .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, + + .triggerCombiner = supervisorAlways, + + .blockerCombiner = supervisorNever, + }, +}; + +static SupervisorStateTransition_t transitionsLocked[] = { + { + .newState = supervisorStateLocked, + + .triggerCombiner = supervisorNever, + + .blockerCombiner = supervisorAlways, + }, +}; + +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(transitionsLocked)}, +}; +static_assert(sizeof(transitionLists) / sizeof(transitionLists[0]) == supervisorState_NrOfStates); + + +bool supervisorStateMachineInit() { + if (sizeof(supervisorState_t) != sizeof(transitionLists)) { + return false; + } + + 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 supervisorConditionBits_t negRequirements, const SupervisorConditionCombiner_t combiner) { + bool result = false; + + switch(combiner) { + case supervisorAll: + result = areAllSet(conditions, requirements) && !isAnySet(conditions, negRequirements); + break; + case supervisorAny: + result = isAnySet(conditions, requirements) || !areAllSet(conditions, negRequirements); + 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 bool isTriggerMatch = areConditionsMet(conditions, transitionDef->triggers, transitionDef->negatedTriggers, transitionDef->triggerCombiner); + const bool isBlockerMatch = areConditionsMet(conditions, transitionDef->blockers, transitionDef->negatedBlockers, transitionDef->blockerCombiner); + + const bool isStateTransitionValid = isTriggerMatch && !isBlockerMatch; + if (isStateTransitionValid) { + newState = transitionDef->newState; + break; + } + } + return newState; +} + +supervisorState_t supervisorStateUpdate(const supervisorState_t currentState, const supervisorConditionBits_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/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 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 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..7d46d51562 --- /dev/null +++ b/test/modules/src/test_supervisor_state_machine.c @@ -0,0 +1,714 @@ +// 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 supervisorConditionBits_t triggerBitField, const SupervisorStateTransitionList_t* transitions); + +// Helpers +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 +} + +void tearDown(void) { + // Empty +} + +void testTransitionWithNoConditionsTriggerAlways(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 = supervisorNever; + + // Test + // Assert + 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; + + 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionOneRequiredConditionNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; + + 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, 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; + + 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, 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 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiRequiredConditionsOneMissing(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 = 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 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 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; + + 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionOneProhibitedConditionMet(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_CHARGER_CONNECTED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionOneProhibitedConditionNotMet(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; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + 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 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiProhibitedConditionsMet(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_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + assertStateTransition(conditions, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiProhibitedConditionsOneNotMet(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_CHARGER_CONNECTED | SUPERVISOR_CB_IS_TUMBLED; + supervisorConditionBits_t negatedBlockers = SUPERVISOR_CB_NONE; + SupervisorConditionCombiner_t blockerCombiner = supervisorAny; + + // Test + // Assert + 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 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; + + 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiRequiredAndProhibitedConditionsOneRequiredNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_ARMED; + + 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, 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 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testTransitionMultiRequiredAndProhibitedConditionsMultipleNotMet(void) { + // Fixture + supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | 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, triggers, negatedTriggers, triggerCombiner, blockers, negatedBlockers, blockerCombiner); +} + +void testFirstValidTransitionIsChosen(void) { + // Fixture + const supervisorConditionBits_t conditions = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP; + + const supervisorState_t currentState = supervisorStateFlying; + const supervisorState_t expected = supervisorStateExceptFreeFall; + + SupervisorStateTransition_t transitions[] = { + { + .newState = supervisorStateLanded, + + .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, + + .triggers = SUPERVISOR_CB_IS_TUMBLED, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, + }, + { + .newState = supervisorStatePreFlChecksNotPassed, + + .triggers = SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP, + .negatedTriggers = SUPERVISOR_CB_NONE, + .triggerCombiner = supervisorAll, + + .blockerCombiner = supervisorNever, + } + }; + + 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(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, + + .triggers = triggers, + .negatedTriggers = negatedTriggers, + .triggerCombiner = triggerCombiner, + + .blockers = blockers, + .negatedBlockers = negatedBlockers, + .blockerCombiner = blockerCombiner, + } + }; + + 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(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 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)); +}