From 098ef140a6393efc1af877cdfdead4de8162a75b Mon Sep 17 00:00:00 2001 From: dario Date: Tue, 7 Nov 2023 12:37:53 +0100 Subject: [PATCH] Added changes for potential statemachine rework --- .settings/language.settings.xml | 4 +- App/Src/startup.cpp | 32 --------- Libs/sta-core | 2 +- Tacos/include/sta/tacos/statemachine.hpp | 73 ++++++++++--------- Tacos/src/statemachine.cpp | 89 +++++++++++------------- 5 files changed, 82 insertions(+), 118 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index f807aa8..13cbd4a 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/App/Src/startup.cpp b/App/Src/startup.cpp index 884a1f3..9b5186a 100644 --- a/App/Src/startup.cpp +++ b/App/Src/startup.cpp @@ -24,38 +24,6 @@ namespace sta { void onStatemachineInit() { - Statemachine::instance()->setStateFunction([](uint16_t state) -> uint16_t { - // ###### Implement your state transition function here! ###### - - /** - * This function should return some state within 0 and STA_TACOS_NUM_STATES (specified in the config.hpp). - * Return the same state as given as the argument for no state transition. Make sure, that this function is not - * computationally expensive. - */ - - return state; - }); - - Statemachine::instance()->setTimerFunction([](uint16_t state, uint16_t lastState, uint32_t event) { - // ###### Set the failsafe and lockout timers here ###### - - /** - * This function is called after a state transition from lastState to state. Event corresponds to EventFlag enum in the Statemachine - * and encode how the state change happened. - * - * Setting a timer here is fully optional and depends on your system's specifications. - */ - - uint32_t someFailsafeMillis = 42; - uint16_t someNextState = STA_TACOS_INITIAL_STATE; - uint32_t someLockoutMillis = 21; - - // Start the failsafe timer to force a state transition to someNextState after someFailsafeMillis milliseconds. - Statemachine::instance()->setFailsafeTimer(someFailsafeMillis, someNextState); - - // Start the lockout timer to block a state transition for the first someLockoutMillis milliseconds. - Statemachine::instance()->setLockoutTimer(someLockoutMillis); - }); } diff --git a/Libs/sta-core b/Libs/sta-core index 4da1f0b..71170d4 160000 --- a/Libs/sta-core +++ b/Libs/sta-core @@ -1 +1 @@ -Subproject commit 4da1f0bb7dd48f89e9dd5ee0ec181638894e55e2 +Subproject commit 71170d4cea428fffb05fc2666201d390ff411fc4 diff --git a/Tacos/include/sta/tacos/statemachine.hpp b/Tacos/include/sta/tacos/statemachine.hpp index bf5fa4c..99b444a 100644 --- a/Tacos/include/sta/tacos/statemachine.hpp +++ b/Tacos/include/sta/tacos/statemachine.hpp @@ -20,6 +20,8 @@ # define STA_TACOS_INITIAL_STATE 0 #endif +#define STA_TACOS_STATEMACHINE_QUEUE_LENGTH 4 + // State transition happened because of #define STA_TACOS_STATE_CHANGE_NORMAL_FLAG ( 0x1U ) #define STA_TACOS_STATE_CHANGE_FORCED_FLAG ( 0x1U << 1) @@ -29,34 +31,41 @@ #include +#include #include #include #include #include #include +#include namespace sta { namespace tacos { - typedef std::function state_fn; - typedef std::function timer_fn; + enum EventFlags + { + NORMAL = 0x1U, + FORCED = 0x1U << 1, + TIMEOUT = 0x1U << 2, + STARTUP = 0x1U << 3, + ALL = NORMAL | FORCED | TIMEOUT | STARTUP + }; + + struct StateTransition + { + uint16_t from; + uint16_t to; + EventFlags event; + uint32_t lockout; + }; class Statemachine : public TacosThread { public: - enum EventFlags - { - NORMAL = 0x1U, - FORCED = 0x1U << 1, - TIMEOUT = 0x1U << 2, - STARTUP = 0x1U << 3, - ALL = NORMAL | FORCED | TIMEOUT | STARTUP - }; - /** * @brief The global event signaling a state change. */ @@ -84,29 +93,23 @@ namespace sta uint16_t getCurrentState() const; /** - * @brief Registers a new state transition function. This is a function for the user to implement state transition rules. + * @brief Request a state transition from a state to another. + * + * @param from The state which we want to leave. This is used to filter out obsolete transitions. + * @param to The state to transition to. + * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions. */ - void setStateFunction(state_fn func); + void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0); /** - * @brief Registers a new timer function. This is a function for the user to set the timers after a state change. + * @brief Request a state transition after a given time has passed. + * + * @param from The state which we want to leave. This is used to filter out obsolete transitions. + * @param to The state to transition to. + * @param millis the number of milliseconds to wait before triggering the transition. + * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions. */ - void setTimerFunction(timer_fn func); - - /** - * @brief Starts the failsafe timer for the desired duration and the desired next state. - */ - void setFailsafeTimer(uint32_t millis, uint16_t nextState); - - /** - * @brief Starts the lockoutTimer for the desired duration. - */ - void setLockoutTimer(uint32_t millis); - - /** - * @brief this function call allows forced state transitions from external and internal sources. - */ - void forceStateTransition(uint16_t state); + void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0); void init() override; void func() override; @@ -133,15 +136,19 @@ namespace sta ~Statemachine() {} + private: + /** + * @brief Starts the lockoutTimer for the desired duration. + */ + void setLockoutTimer(uint32_t millis); + private: uint16_t currentState_; - uint16_t failsafeState_; RtosTimer lockoutTimer_; RtosTimer failsafeTimer_; - state_fn transitionFunc_; - timer_fn timerFunc_; + RtosQueue queue_; }; } // namespace tacos } // namespace sta diff --git a/Tacos/src/statemachine.cpp b/Tacos/src/statemachine.cpp index c7daf0f..aade590 100644 --- a/Tacos/src/statemachine.cpp +++ b/Tacos/src/statemachine.cpp @@ -21,7 +21,8 @@ namespace sta lockoutTimer_{[](void *){}, nullptr}, failsafeTimer_{[](void *){}, nullptr}, transitionFunc_{[](uint16_t) -> uint16_t { return Statemachine::instance()->getCurrentState(); }}, - timerFunc_{[](uint16_t, uint16_t, uint16_t) {}} + timerFunc_{[](uint16_t, uint16_t, uint16_t) {}}, + queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH} { STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES); } @@ -33,31 +34,40 @@ namespace sta void Statemachine::func() { - if (!lockoutTimer_.isRunning()) + // Wait for a message to be added to the queue. + StateTransition transition; + queue_.get(&transition, osWaitForever); + + // Check if the transition isn't blocked and is legal. + if (!lockoutTimer_.isRunning() && transition.from == currentState_) { - uint16_t next = transitionFunc_(currentState_); + STA_ASSERT(transition.to < STA_TACOS_NUM_STATES); - STA_ASSERT(next < STA_TACOS_NUM_STATES); + // Perform the transition and notify the threads. The event flags are set + // here in order to allow threads to react immediately. + currentState_ = transition.to; + Statemachine::stateChangeEvent.set(transition.event); + Statemachine::stateChangeEvent.clear(EventFlags::ALL); - // Check if a state change is desired. Block if the lockoutTimer is still running. - if (next != currentState_) + if (failsafeTimer_.isRunning()) { - if (failsafeTimer_.isRunning()) - { - failsafeTimer_.stop(); - } + failsafeTimer_.stop(); + } - // Call user space code to set the timers again. - timerFunc_(next, currentState_, EventFlags::NORMAL); + // Start the lockout timer if requested. + if (transition.lockout != NULL) + { + setLockoutTimer(transition.lockout); + } - // Update the state and trigger the global state changed event. - currentState_ = next; - Statemachine::stateChangeEvent.set(EventFlags::NORMAL); - Statemachine::stateChangeEvent.clear(EventFlags::ALL); + // Start the failsafe timer if requested. + if (transition.failsafe != NULL) + { + std::tie(millis, state) = transition.failsafe; + + setFailsafeTimer(millis, state); } } - - osThreadYield(); } uint16_t Statemachine::getCurrentState() const @@ -65,27 +75,25 @@ namespace sta return currentState_; } - void Statemachine::setStateFunction(state_fn func) + void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */) { - STA_ASSERT(func != nullptr); + StateTransition transition; + transition.from = from; + transition.to = to; + transition.event = EventFlags::NORMAL; + transition.lockout = lockout; - transitionFunc_ = func; + // TODO: For how should we wait here? + queue_.put(transition); } - void Statemachine::setTimerFunction(timer_fn func) - { - STA_ASSERT(func != nullptr); - - timerFunc_ = func; - } - - void Statemachine::setFailsafeTimer(uint32_t millis, uint16_t nextState) + void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */) { STA_ASSERT(nextState < STA_TACOS_NUM_STATES); STA_ASSERT(!failsafeTimer_.isRunning()); - failsafeTimer_.setCallback([nextState](void* arg) { - Statemachine::instance()->forceStateTransition(nextState); + failsafeTimer_.setCallback([from, to, lockout](void* arg) { + Statemachine::requestStateTransition(from, to, lockout); }, &failsafeState_); failsafeTimer_.start(millis); @@ -99,25 +107,6 @@ namespace sta lockoutTimer_.start(millis); } - void Statemachine::forceStateTransition(uint16_t state) - { - if (failsafeTimer_.isRunning()) - { - failsafeTimer_.stop(); - } - - if (lockoutTimer_.isRunning()) - { - lockoutTimer_.stop(); - } - - timerFunc_(state, currentState_, EventFlags::FORCED); - currentState_ = state; - - Statemachine::stateChangeEvent.set(EventFlags::FORCED); - Statemachine::stateChangeEvent.clear(EventFlags::ALL); - } - Statemachine* Statemachine::_instance = nullptr; RtosEvent Statemachine::stateChangeEvent;