From d028ecef74e0b69dc5a5be1746df44766f119a09 Mon Sep 17 00:00:00 2001 From: CarlWachter Date: Mon, 17 Jun 2024 08:54:36 +0200 Subject: [PATCH 1/3] feat(state-via-can): state transitions via can are forced to resync state across devices --- include/sta/tacos.hpp | 3 ++- include/sta/tacos/statemachine.hpp | 3 ++- src/can_bus.cpp | 4 ++-- src/statemachine.cpp | 27 +++++++++++++++++++++++---- src/tacos.cpp | 4 ++-- 5 files changed, 31 insertions(+), 10 deletions(-) diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp index b7b6ccf..8d6fa36 100644 --- a/include/sta/tacos.hpp +++ b/include/sta/tacos.hpp @@ -42,10 +42,11 @@ 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. * * @ingroup tacos_api */ - void setState(uint32_t from, uint32_t to, uint32_t lockout = 0); + void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false); /** * @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 9305316..afa095b 100644 --- a/include/sta/tacos/statemachine.hpp +++ b/include/sta/tacos/statemachine.hpp @@ -162,8 +162,9 @@ 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. */ - void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0); + void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false); /** * @brief Request a state transition after a given time has passed. diff --git a/src/can_bus.cpp b/src/can_bus.cpp index 39f2387..2eca8af 100644 --- a/src/can_bus.cpp +++ b/src/can_bus.cpp @@ -155,8 +155,8 @@ namespace sta { STA_ASSERT(header.payloadLength == 2); - // First byte of payload is the origin state, second byte is the destination state - tacos::setState(payload[0], payload[1]); + // 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); } } // namespace tacos } // namespace sta diff --git a/src/statemachine.cpp b/src/statemachine.cpp index ea07e01..5d92a0e 100644 --- a/src/statemachine.cpp +++ b/src/statemachine.cpp @@ -64,7 +64,7 @@ namespace sta return currentState_; } - void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */) + void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */) { StateTransition transition; transition.from = from; @@ -72,9 +72,28 @@ namespace sta transition.event = EventFlags::NORMAL; transition.lockout = lockout; - // 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); + if (force){ + // 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); + + if (failsafeTimer_.isRunning()) + { + failsafeTimer_.stop(); + } + + // Start the lockout timer if requested. + if (transition.lockout != 0) + { + setLockoutTimer(transition.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); + } } void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */) diff --git a/src/tacos.cpp b/src/tacos.cpp index ca50225..99a9380 100644 --- a/src/tacos.cpp +++ b/src/tacos.cpp @@ -16,9 +16,9 @@ namespace sta return Statemachine::instance()->getCurrentState(); } - void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */) + void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */) { - Statemachine::instance()->requestStateTransition(from, to, lockout); + Statemachine::instance()->requestStateTransition(from, to, lockout, force); } void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */) From dd4c5f34a21003658d5334bf094a9623e023ff66 Mon Sep 17 00:00:00 2001 From: CarlWachter Date: Fri, 23 Aug 2024 15:59:35 +0200 Subject: [PATCH 2/3] feat: handleSysMessage checks allows user to overwrite CAN system behavior --- include/sta/tacos/can_bus.hpp | 13 ++++++++++--- src/can_bus.cpp | 33 +++++++++++++++++++-------------- 2 files changed, 29 insertions(+), 17 deletions(-) diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp index 750292e..27b0086 100644 --- a/include/sta/tacos/can_bus.hpp +++ b/include/sta/tacos/can_bus.hpp @@ -111,10 +111,17 @@ namespace sta }; /** - * @brief Callback function for handling received messages. Intended for state transitions. - */ + * @brief Handle system messages received over the CAN bus. Called as soon as a message is received. If + * the message is a system message, it will be handled here. If the message is not a system message, it will be + * passed to the appropriate thread's queue. + * + * @param header The header of the received message. + * @param payload The payload of the received message. + * + * @return True if the message was a system message. + */ STA_WEAK - void handleSysMessage(CanMsgHeader & header, uint8_t * payload); + bool handleSysMessage(CanMsgHeader & header, uint8_t * payload); } /* namespace tacos */ diff --git a/src/can_bus.cpp b/src/can_bus.cpp index 2eca8af..f870201 100644 --- a/src/can_bus.cpp +++ b/src/can_bus.cpp @@ -63,17 +63,15 @@ namespace sta sysMsg = *canBusSysQueueBuffer_[i]; canBusSysQueueBuffer_[i] = nullptr; - // Append to the correct thread's queue - for (std::shared_ptr thread : Manager::instance()->getActiveThreads()){ - if (thread->getCanID() == sysMsg.header.sid){ - thread->CAN_queue_.put(sysMsg); - break; - } - } + if (!handleSysMessage(sysMsg.header, sysMsg.payload)){ - if(sysMsg.header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){ - // Handle system message - handleSysMessage(sysMsg.header, sysMsg.payload); + // Append to the correct thread's queue + for (std::shared_ptr thread : Manager::instance()->getActiveThreads()){ + if (thread->getCanID() == sysMsg.header.sid){ + thread->CAN_queue_.put(sysMsg); + break; + } + } } } } @@ -148,15 +146,22 @@ namespace sta { namespace tacos { - void handleSysMessage(CanMsgHeader & header, uint8_t * payload) + STA_WEAK + bool handleSysMessage(CanMsgHeader & header, uint8_t * payload) { // This is a weak function that can be overridden by the user, // if they want to handle system messages in a different way, i.e. ignore them - STA_ASSERT(header.payloadLength == 2); + if(header.sid == 0x0){ + 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); + // 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); + + return true; + } + + return false; } } // namespace tacos } // namespace sta From de6615af952c3763d7ddc6472b8e4084b3a03b52 Mon Sep 17 00:00:00 2001 From: CarlWachter Date: Fri, 23 Aug 2024 17:36:50 +0200 Subject: [PATCH 3/3] chore: weak CAN sys message handler uses STA_TACOS_CAN_BUS_SYS_MSG_ID constant --- src/can_bus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/can_bus.cpp b/src/can_bus.cpp index f870201..529581b 100644 --- a/src/can_bus.cpp +++ b/src/can_bus.cpp @@ -152,7 +152,7 @@ namespace sta { // This is a weak function that can be overridden by the user, // if they want to handle system messages in a different way, i.e. ignore them - if(header.sid == 0x0){ + if(header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){ STA_ASSERT(header.payloadLength == 2); // First byte of payload is the origin state, second byte is the destination state. Transition is forced