mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/TACOS.git
synced 2025-06-10 16:45:59 +00:00
321 lines
8.0 KiB
C++
321 lines
8.0 KiB
C++
/*
|
|
* statemachine.hpp
|
|
*
|
|
* Created on: Sep 14, 2023
|
|
* Author: Dario
|
|
*/
|
|
|
|
#ifndef INCLUDE_TACOS_STATEMACHINE_HPP_
|
|
#define INCLUDE_TACOS_STATEMACHINE_HPP_
|
|
|
|
#include <sta/config.hpp>
|
|
|
|
#if !defined(STA_TACOS_NUM_STATES) && !defined(DOXYGEN)
|
|
# error "Number of states wasn't defined in config.hpp"
|
|
#else
|
|
|
|
#if !defined(STA_TACOS_INITIAL_STATE) && !defined(DOXYGEN)
|
|
# define STA_TACOS_INITIAL_STATE 0
|
|
#endif
|
|
|
|
/**
|
|
* @defgroup tacos_statemachine Statemachine Task
|
|
* @ingroup tacos
|
|
* @brief Statemachine task for TACOS.
|
|
*/
|
|
|
|
/**
|
|
* @brief The maximum number of state transitions that can be queued.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
#define STA_TACOS_STATEMACHINE_QUEUE_LENGTH 4
|
|
|
|
/**
|
|
* @ingroup tacos_statemachine
|
|
* @{
|
|
*/
|
|
|
|
// State transition happened because of
|
|
/**
|
|
* @brief State change due to requested Transition.
|
|
*/
|
|
#define STA_TACOS_STATE_CHANGE_NORMAL_FLAG ( 0x1U )
|
|
/**
|
|
* @brief State change due to forced Transition.
|
|
*/
|
|
#define STA_TACOS_STATE_CHANGE_FORCED_FLAG ( 0x1U << 1)
|
|
/**
|
|
* @brief State change due to timeout.
|
|
*/
|
|
#define STA_TACOS_STATE_CHANGE_TIMEOUT ( 0x1U << 2)
|
|
/**
|
|
* @brief State change due to startup.
|
|
*/
|
|
#define STA_TACOS_STATE_CHANGE_STARTUP_FLAG ( 0x1U << 3)
|
|
/**
|
|
* @brief State change due to any reason.
|
|
*/
|
|
#define STA_TACOS_STATE_CHANGE_ALL_FLAG ( 0x15U )
|
|
|
|
/** @} */
|
|
|
|
#include <sta/tacos/thread.hpp>
|
|
#include <sta/rtos/queue.hpp>
|
|
#include <sta/rtos/timer.hpp>
|
|
#include <sta/rtos/event.hpp>
|
|
#include <sta/event.hpp>
|
|
#include <sta/debug/assert.hpp>
|
|
#include <sta/lang.hpp>
|
|
|
|
#include <functional>
|
|
#include <tuple>
|
|
#include <list>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <set>
|
|
#include <algorithm>
|
|
|
|
|
|
/**
|
|
* @brief Addresses all states from 0 to STA_TACOS_NUM_STATES-1.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
#define ALL_STATES std::set<uint16_t>{[]{ std::set<uint16_t> states; for (uint16_t i = 0; i < STA_TACOS_NUM_STATES; ++i) states.insert(i); return states; }() }
|
|
|
|
/**
|
|
* @brief Shorthand to make own sets of states. Primarily for exlcusion via "-" operator.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
#define state_set std::set<uint16_t>
|
|
|
|
namespace sta
|
|
{
|
|
namespace tacos
|
|
{
|
|
/**
|
|
* @brief Event flags for state transitions.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
enum EventFlags
|
|
{
|
|
NORMAL = 0x1U,
|
|
FORCED = 0x1U << 1,
|
|
TIMEOUT = 0x1U << 2,
|
|
STARTUP = 0x1U << 3,
|
|
ALL = NORMAL | FORCED | TIMEOUT | STARTUP
|
|
};
|
|
|
|
/**
|
|
* @brief State transition structure.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
struct StateTransition
|
|
{
|
|
/// Origin of transition
|
|
uint16_t from;
|
|
/// Destination of transition
|
|
uint16_t to;
|
|
/// Event that triggered the transition
|
|
EventFlags event;
|
|
/// Lockout time after transition
|
|
uint32_t lockout;
|
|
/// Whether to publish the transition via CAN.
|
|
bool publish = false;
|
|
};
|
|
|
|
/**
|
|
* @brief Statemachine implementation for Tacos.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
class Statemachine : public TacosThread
|
|
{
|
|
public:
|
|
/**
|
|
* @brief The global event signaling a state change.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
static RtosEvent stateChangeEvent;
|
|
|
|
/**
|
|
* @brief Getter function for the singleton instance.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
static Statemachine* instance()
|
|
{
|
|
static CGuard g;
|
|
|
|
if (!_instance)
|
|
{
|
|
// Create the statemachine singleton instance.
|
|
Statemachine::_instance = new Statemachine();
|
|
}
|
|
|
|
return _instance;
|
|
}
|
|
|
|
/**
|
|
* @brief Returns the statemachine's current state.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
uint16_t getCurrentState() const;
|
|
|
|
/**
|
|
* @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 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.
|
|
*
|
|
* @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 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
|
|
*/
|
|
void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true);
|
|
|
|
/**
|
|
* @brief Register a thread to be managed by the statemachine.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
void registerThread(std::shared_ptr<TacosThread> thread, std::set<uint16_t> states);
|
|
|
|
/**
|
|
* @brief Get the Active Threads object
|
|
*
|
|
* @return std::vector<std::shared_ptr<TacosThread>>
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
std::vector<std::shared_ptr<TacosThread>> getActiveThreads();
|
|
|
|
void init() override;
|
|
void func() override;
|
|
|
|
private:
|
|
static Statemachine * _instance;
|
|
|
|
class CGuard
|
|
{
|
|
public:
|
|
~CGuard()
|
|
{
|
|
if( NULL != Statemachine::_instance )
|
|
{
|
|
delete Statemachine::_instance;
|
|
Statemachine::_instance = NULL;
|
|
}
|
|
}
|
|
};
|
|
|
|
Statemachine();
|
|
|
|
Statemachine(const Statemachine&);
|
|
|
|
~Statemachine() {}
|
|
|
|
private:
|
|
/**
|
|
* @brief Starts the lockoutTimer for the desired duration.
|
|
*
|
|
* @param millis The duration of the timer in milliseconds.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
void setLockoutTimer(uint32_t millis);
|
|
|
|
/**
|
|
* @brief Forces only threads of current state to run.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
void updateThreads();
|
|
|
|
/**
|
|
* @brief Starts all threads which should be running in the given state. Does nothing if the state is already running.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
void startThreads(uint16_t state);
|
|
|
|
/**
|
|
* @brief Stops all threads which should not be running in the given state.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
void stopThreads(uint16_t state);
|
|
|
|
private:
|
|
uint16_t currentState_;
|
|
|
|
RtosTimer lockoutTimer_;
|
|
RtosTimer failsafeTimer_;
|
|
|
|
RtosQueue<StateTransition> queue_;
|
|
|
|
/**
|
|
* @brief Pointers to all threads which are managed by the statemachine.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
std::vector<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
|
|
};
|
|
|
|
/**
|
|
* @brief Callback that is called when a state transition occurs.
|
|
*
|
|
* @param from The state we transitioned from.
|
|
* @param to The state we transitioned to.
|
|
* @param lockout The lockout time after the transition.
|
|
*
|
|
* @ingroup tacos_statemachine
|
|
*/
|
|
STA_WEAK
|
|
void onStateTransition(uint16_t from, uint16_t to, uint32_t lockout){}
|
|
} // namespace tacos
|
|
} // namespace sta
|
|
|
|
#endif // STA_TACOS_NUM_STATES
|
|
|
|
/// Overwrite "-" operator for set.
|
|
template <typename T>
|
|
std::set<T> operator-(const std::set<T>& set1, const std::set<T>& set2) {
|
|
std::set<T> result;
|
|
std::set_difference(set1.begin(), set1.end(), set2.begin(), set2.end(), std::inserter(result, result.begin()));
|
|
return result;
|
|
}
|
|
|
|
#endif /* INCLUDE_TACOS_STATEMACHINE_HPP_ */
|