mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/TACOS.git
synced 2025-06-10 16:45:59 +00:00
127 lines
3.3 KiB
C++
127 lines
3.3 KiB
C++
/*
|
|
* statemachine.cpp
|
|
*
|
|
* Created on: 21 Sep 2023
|
|
* Author: Dario
|
|
*/
|
|
|
|
#include <sta/tacos/statemachine.hpp>
|
|
|
|
#include <sta/debug/debug.hpp>
|
|
|
|
|
|
namespace sta
|
|
{
|
|
namespace tacos
|
|
{
|
|
Statemachine::Statemachine()
|
|
: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY, STA_TACOS_STATEMACHINE_STACK_SIZE},
|
|
currentState_{STA_TACOS_INITIAL_STATE},
|
|
lockoutTimer_{[](void *){}, nullptr},
|
|
failsafeTimer_{[](void *){}, nullptr},
|
|
queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH}
|
|
{
|
|
STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
|
|
}
|
|
|
|
void Statemachine::init()
|
|
{
|
|
|
|
}
|
|
|
|
void Statemachine::func()
|
|
{
|
|
// 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_)
|
|
{
|
|
STA_ASSERT(transition.to < 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);
|
|
|
|
if (failsafeTimer_.isRunning())
|
|
{
|
|
failsafeTimer_.stop();
|
|
}
|
|
|
|
// Start the lockout timer if requested.
|
|
if (transition.lockout != 0)
|
|
{
|
|
setLockoutTimer(transition.lockout);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint16_t Statemachine::getCurrentState() const
|
|
{
|
|
return currentState_;
|
|
}
|
|
|
|
void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */)
|
|
{
|
|
StateTransition transition;
|
|
transition.from = from;
|
|
transition.to = to;
|
|
transition.event = EventFlags::NORMAL;
|
|
transition.lockout = lockout;
|
|
|
|
// 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;
|
|
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 */)
|
|
{
|
|
STA_ASSERT(to < STA_TACOS_NUM_STATES);
|
|
|
|
failsafeTimer_.setCallback([from, to, lockout](void* arg) {
|
|
Statemachine::instance()->requestStateTransition(from, to, lockout);
|
|
}, NULL);
|
|
|
|
failsafeTimer_.start(millis);
|
|
}
|
|
|
|
void Statemachine::setLockoutTimer(uint32_t millis)
|
|
{
|
|
STA_ASSERT(!lockoutTimer_.isRunning());
|
|
|
|
lockoutTimer_.setCallback([](void *) { }, nullptr);
|
|
lockoutTimer_.start(millis);
|
|
}
|
|
|
|
Statemachine* Statemachine::_instance = nullptr;
|
|
|
|
RtosEvent Statemachine::stateChangeEvent;
|
|
} // namespace tacos
|
|
} // namespace sta
|
|
|
|
|
|
|