ALPAKA/TACOS
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/TACOS.git synced 2025-08-02 08:41:54 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added state publishing to TACOS state transition

Browse Source
This commit is contained in:
dario 2024-10-04 13:32:30 +02:00
parent c784e37eab
commit d1f7063912
4 changed files with 34 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/sta/tacos.hpp
View File

@ -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
}

6
include/sta/tacos/statemachine.hpp
View File

@ -120,6 +120,8 @@ namespace sta
EventFlags event;
/// Lockout time after transition
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;

24
src/statemachine.cpp
View File

@ -5,6 +5,7 @@
* Author: Dario
*/
#include <sta/tacos.hpp>
#include <sta/tacos/statemachine.hpp>
#include <sta/debug/debug.hpp>
@ -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);

12
src/tacos.cpp
View File

@ -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