From d1f7063912a4c4d588552db17360f62be4f1430e Mon Sep 17 00:00:00 2001 From: dario Date: Fri, 4 Oct 2024 13:32:30 +0200 Subject: [PATCH] Added state publishing to TACOS state transition --- include/sta/tacos.hpp | 8 ++++---- include/sta/tacos/statemachine.hpp | 8 +++++--- src/statemachine.cpp | 24 +++++++++++++++++++----- src/tacos.cpp | 12 ++++++------ 4 files changed, 34 insertions(+), 18 deletions(-) diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp index 8d6fa36..aa5d408 100644 --- a/include/sta/tacos.hpp +++ b/include/sta/tacos.hpp @@ -46,7 +46,7 @@ namespace sta * * @ingroup tacos_api */ - void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false); + void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true); /** * @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed. @@ -58,7 +58,7 @@ namespace sta * * @ingroup tacos_api */ - void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0); + void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true); /** * @brief Register a new thread to be run by TACOS. @@ -97,13 +97,13 @@ namespace sta * * @param from The state 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 timeout An optional timeout for the CAN communication * * @return bool True if the message was sent successfully. * * @ingroup tacos_api */ - bool publishState(uint32_t from, uint32_t to, uint32_t lockout = 0); + bool publishState(uint32_t from, uint32_t to, uint32_t timeout = 0); #endif // STA_TACOS_CAN_BUS_ENABLED } // namespace tacos } diff --git a/include/sta/tacos/statemachine.hpp b/include/sta/tacos/statemachine.hpp index afa095b..452809b 100644 --- a/include/sta/tacos/statemachine.hpp +++ b/include/sta/tacos/statemachine.hpp @@ -119,7 +119,9 @@ namespace sta /// Event that triggered the transition EventFlags event; /// Lockout time after transition - uint32_t lockout; + uint32_t lockout; + /// Whether to publish the transition via CAN. + bool publish = false; }; /** @@ -164,7 +166,7 @@ namespace sta * @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. */ - void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false); + void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true); /** * @brief Request a state transition after a given time has passed. @@ -174,7 +176,7 @@ namespace sta * @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 requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0); + void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true); void init() override; void func() override; diff --git a/src/statemachine.cpp b/src/statemachine.cpp index 6df4e93..d970a57 100644 --- a/src/statemachine.cpp +++ b/src/statemachine.cpp @@ -5,6 +5,7 @@ * Author: Dario */ +#include #include #include @@ -40,9 +41,16 @@ 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); +#endif // STA_TACOS_CAN_BUS_ENABLED + // Perform the transition and notify the threads. The event flags are set // here in order to allow threads to react immediately. currentState_ = transition.to; + + // Send a system-wide notification for the state transition. Statemachine::stateChangeEvent.set(transition.event); Statemachine::stateChangeEvent.clear(EventFlags::ALL); @@ -64,19 +72,25 @@ namespace sta return currentState_; } - void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */) + void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */, bool publish /* = true */) { StateTransition transition; transition.from = from; transition.to = to; transition.event = EventFlags::NORMAL; 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_){ // Perform the transition and notify the threads. The event flags are set // here in order to allow threads to react immediately. currentState_ = transition.to; + +#ifdef STA_TACOS_CAN_BUS_ENABLED + tacos::publishState(transition.from, transition.to, transition.lockout); +#endif // STA_TACOS_CAN_BUS_ENABLED + Statemachine::stateChangeEvent.set(transition.event); Statemachine::stateChangeEvent.clear(EventFlags::ALL); @@ -90,19 +104,19 @@ namespace sta { setLockoutTimer(transition.lockout); } - }else{ + } 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); } } - void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */) + void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = true */) { STA_ASSERT(to < STA_TACOS_NUM_STATES); - failsafeTimer_.setCallback([from, to, lockout](void* arg) { - Statemachine::instance()->requestStateTransition(from, to, lockout); + failsafeTimer_.setCallback([from, to, lockout, publish](void* arg) { + Statemachine::instance()->requestStateTransition(from, to, lockout, false, publish); }, NULL); failsafeTimer_.start(millis); diff --git a/src/tacos.cpp b/src/tacos.cpp index 99a9380..f1c615f 100644 --- a/src/tacos.cpp +++ b/src/tacos.cpp @@ -16,14 +16,14 @@ namespace sta return Statemachine::instance()->getCurrentState(); } - void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */) + void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */, bool publish /* = false */) { - Statemachine::instance()->requestStateTransition(from, to, lockout, force); + Statemachine::instance()->requestStateTransition(from, to, lockout, force, publish); } - void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */) + void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = false */) { - Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout); + Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout, publish); } #ifdef STA_TACOS_CAN_BUS_ENABLED @@ -31,7 +31,7 @@ namespace sta return CanBus::instance()->queueCanBusMsg(msg, timeout); } - bool publishState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */){ + bool publishState(uint32_t from, uint32_t to, uint32_t timeout /* = 0 */){ CanSysMsg msg; msg.header.sid = STA_TACOS_CAN_BUS_SYS_MSG_ID; msg.header.payloadLength = 2; @@ -41,7 +41,7 @@ namespace sta msg.header.eid = 0; msg.header.format = 0; - return CanBus::instance()->queueCanBusMsg(msg, lockout); + return CanBus::instance()->queueCanBusMsg(msg, timeout); } #endif // STA_TACOS_CAN_BUS_ENABLED } // namespace tacos