From ca3f303293b2c9b8c15b3ac5ad3dd62d786f0dba Mon Sep 17 00:00:00 2001 From: CarlWachter Date: Sun, 3 Nov 2024 13:42:25 +0100 Subject: [PATCH] cleanup: separate functions for requesting and forcing state transitions, fix publishing behavior --- include/sta/tacos.hpp | 16 ++++++++++-- include/sta/tacos/statemachine.hpp | 17 +++++++++++-- src/can_bus.cpp | 2 +- src/statemachine.cpp | 40 +++++++++++++++++++----------- src/tacos.cpp | 9 +++++-- 5 files changed, 62 insertions(+), 22 deletions(-) diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp index fa88c28..99fbc2c 100644 --- a/include/sta/tacos.hpp +++ b/include/sta/tacos.hpp @@ -41,11 +41,23 @@ namespace sta * @param from The start we want to transition from. * @param to The state we want to transition to. * @param lockout An optional timer blocking state transition for a given time. - * @param force If true, the state transition will be executed regardless of the current state. + * @param publish If true, the state transition will be published via CAN. * * @ingroup tacos_api */ - void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = false); + void requestState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true); + + /** + * @brief Request a state transition. Invalid state transitions will be dismissed. + * + * @param from The start we want to transition from. + * @param to The state we want to transition to. + * @param lockout An optional timer blocking state transition for a given time. + * @param publish If true, the state transition will be published via CAN. + * + * @ingroup tacos_api + */ + void forceState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true); /** * @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed. diff --git a/include/sta/tacos/statemachine.hpp b/include/sta/tacos/statemachine.hpp index 07fbdea..d9fd517 100644 --- a/include/sta/tacos/statemachine.hpp +++ b/include/sta/tacos/statemachine.hpp @@ -174,11 +174,23 @@ namespace sta * @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. - * @param force If true, the state transition will be executed regardless of the current state. + * @param publish If true, the state transition will be published via CAN. * * @ingroup tacos_statemachine */ - void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true); + void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true); + + /** + * @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. + * @param publish If true, the state transition will be published via CAN. + * + * @ingroup tacos_statemachine + */ + void forceStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool publish = true); /** * @brief Request a state transition after a given time has passed. @@ -187,6 +199,7 @@ namespace sta * @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. + * @param publish If true, the state transition will be published via CAN. * * @ingroup tacos_statemachine */ diff --git a/src/can_bus.cpp b/src/can_bus.cpp index 06ad880..bc4efda 100644 --- a/src/can_bus.cpp +++ b/src/can_bus.cpp @@ -160,7 +160,7 @@ namespace sta STA_ASSERT(header.payloadLength == 2); // First byte of payload is the origin state, second byte is the destination state. Transition is forced - tacos::setState(payload[0], payload[1], 0, true); + tacos::forceState(payload[0], payload[1], 0, true); return true; } diff --git a/src/statemachine.cpp b/src/statemachine.cpp index 2d10a67..03c5f51 100644 --- a/src/statemachine.cpp +++ b/src/statemachine.cpp @@ -44,8 +44,11 @@ namespace sta STA_ASSERT(transition.to < STA_TACOS_NUM_STATES); #ifdef STA_TACOS_CAN_BUS_ENABLED - // Publish the state via CAN bus. - tacos::publishState(transition.from, transition.to, 0); + if (transition.publish) + { + // Publish the state via CAN bus. + tacos::publishState(transition.from, transition.to, 0); + } #endif // STA_TACOS_CAN_BUS_ENABLED // Perform the transition and notify the threads. The event flags are set @@ -84,7 +87,7 @@ namespace sta return currentState_; } - void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */, bool publish /* = true */) + void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */) { StateTransition transition; transition.from = from; @@ -93,17 +96,28 @@ namespace sta transition.lockout = lockout; transition.publish = publish; - // Force the transition if requested, but only if the requested state is different from the current one. - if (force && transition.to != currentState_){ + // Try to add a state transition request to the queue. Don't wait if another + // thread is already requesting a state change. + queue_.put(transition, 0); + } + + void Statemachine::forceStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */) + { + // Force the transition, but only if the requested state is different from the current one. + if (to != currentState_){ // Perform the transition and notify the threads. The event flags are set // here in order to allow threads to react immediately. - currentState_ = transition.to; + currentState_ = to; #ifdef STA_TACOS_CAN_BUS_ENABLED - tacos::publishState(transition.from, transition.to, transition.lockout); + if (publish) + { + // Publish the state via CAN bus. + tacos::publishState(from, to, 0); + } #endif // STA_TACOS_CAN_BUS_ENABLED - Statemachine::stateChangeEvent.set(transition.event); + Statemachine::stateChangeEvent.set(EventFlags::NORMAL); Statemachine::stateChangeEvent.clear(EventFlags::ALL); if (failsafeTimer_.isRunning()) @@ -112,14 +126,10 @@ namespace sta } // Start the lockout timer if requested. - if (transition.lockout != 0) + if (lockout != 0) { - setLockoutTimer(transition.lockout); + setLockoutTimer(lockout); } - } else { - // Try to add a state transition request to the queue. Don't wait if another - // thread is already requesting a state change. - queue_.put(transition, 0); } } @@ -128,7 +138,7 @@ namespace sta STA_ASSERT(to < STA_TACOS_NUM_STATES); failsafeTimer_.setCallback([from, to, lockout, publish](void* arg) { - Statemachine::instance()->requestStateTransition(from, to, lockout, false, publish); + Statemachine::instance()->requestStateTransition(from, to, lockout, publish); }, NULL); failsafeTimer_.start(millis); diff --git a/src/tacos.cpp b/src/tacos.cpp index f1c615f..94491f0 100644 --- a/src/tacos.cpp +++ b/src/tacos.cpp @@ -16,9 +16,14 @@ namespace sta return Statemachine::instance()->getCurrentState(); } - void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */, bool publish /* = false */) + void requestState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */) { - Statemachine::instance()->requestStateTransition(from, to, lockout, force, publish); + Statemachine::instance()->requestStateTransition(from, to, lockout, publish); + } + + void forceState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool publish /* = true */) + { + Statemachine::instance()->forceStateTransition(from, to, lockout, publish); } void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = false */)