diff --git a/Inc/C++Utilities/StaticVector.hpp b/Inc/C++Utilities/StaticVector.hpp new file mode 100644 index 000000000..76efeaa11 --- /dev/null +++ b/Inc/C++Utilities/StaticVector.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include "ErrorHandler/ErrorHandler.hpp" + +template +class StaticVector { + private: + std::array data{}; + size_t size_ = 0; + +public: + constexpr StaticVector() = default; + + template + constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} + + constexpr bool operator==(const StaticVector&) const = default; + + constexpr void push_back(const T& value) + { + if (size_ >= Capacity) + { + ErrorHandler("StaticVector capacity exceeded"); + return; + } + data[size_] = value; + size_++; + } + + constexpr auto begin() { return data.begin(); } + constexpr auto begin() const { return data.begin(); } + constexpr auto end() { return data.begin() + size_; } + constexpr auto end() const { return data.begin() + size_; } + + constexpr const std::array& get_array() const { return data; } + constexpr size_t size() const { return size_; } + constexpr T* get_data() { return data.data(); } + constexpr const T* get_data() const { return data.data(); } + constexpr T& operator[](size_t i) { return data[i]; } + constexpr const T& operator[](size_t i) const { return data[i]; } + constexpr bool contains(const T& value) const + { + for (size_t i = 0; i < size_; ++i) + { + if (data[i] == value) + { + return true; + } + } + return false; + } +}; + diff --git a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp index d4a7e708e..4beb10945 100644 --- a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp +++ b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp @@ -42,7 +42,7 @@ class ProtectionManager { static void set_id(Boards::ID id); - static void link_state_machine(StateMachine& general_state_machine, + static void link_state_machine(IStateMachine& general_state_machine, state_id fault_id); template low_frequency_protections; static vector high_frequency_protections; - static StateMachine* general_state_machine; + static IStateMachine* general_state_machine; static state_id fault_state_id; static Notification fault_notification; diff --git a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp index 5746f00e4..7341d204c 100644 --- a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp @@ -1,19 +1,20 @@ #pragma once #include "StateMachine/StateOrder.hpp" +template class HeapStateOrder : public HeapOrder{ public: - StateMachine& state_machine; - StateMachine::state_id state; + StateMachineType& state_machine; + typename StateMachineType::state_id state; template - HeapStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values) : HeapOrder(id,callback,values...), + HeapStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : HeapOrder(id,callback,values...), state_machine(state_machine), state(state) { if(not state_machine.get_states().contains(state)){ ErrorHandler("State Machine does not contain state, cannot add StateOrder"); return; } - else state_machine.get_states()[state].add_state_order(id); + else state_machine.get_states()[static_cast(state)].add_state_order(id); orders[id] = this; } @@ -24,4 +25,7 @@ class HeapStateOrder : public HeapOrder{ void parse(OrderProtocol* socket, uint8_t* data)override{ if(state_machine.is_on && state_machine.current_state == state) HeapOrder::parse(data); } -}; \ No newline at end of file +}; + +template +HeapStateOrder(uint16_t, void(*)(void), StateMachineType&, typename StateMachineType::state_id, Types*...) -> HeapStateOrder; diff --git a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp index 40c845d7b..9955d6e7b 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp @@ -1,18 +1,18 @@ #pragma once #include "StateMachine/StateOrder.hpp" -template requires NotCallablePack +template requires NotCallablePack class StackStateOrder : public StackOrder{ public: - StateMachine& state_machine; - StateMachine::state_id state; - StackStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values) : StackOrder(id,callback,values...), + StateMachineType& state_machine; + typename StateMachineType::state_id state; + StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : StackOrder(id,callback,values...), state_machine(state_machine), state(state) { if(not state_machine.get_states().contains(state)){ ErrorHandler("State Machine does not contain state, cannot add StateOrder"); return; } - else state_machine.get_states()[state].add_state_order(id); + else state_machine.get_states()[static_cast(state)].add_state_order(id); Order::orders[id] = this; } @@ -26,6 +26,6 @@ class StackStateOrder : public StackOrder{ }; #if __cpp_deduction_guides >= 201606 -template requires NotCallablePack -StackStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values)->StackStateOrder<(!has_container::value)*total_sizeof::value, Types...>; -#endif \ No newline at end of file +template requires NotCallablePack +StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values)->StackStateOrder::value)*total_sizeof::value, Types...>; +#endif diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 1f8fa36c5..790d02b5b 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -1,11 +1,16 @@ -/* - * Created by Alejandro - */ - #pragma once #include "C++Utilities/CppUtils.hpp" +#include "C++Utilities/StaticVector.hpp" #include "ErrorHandler/ErrorHandler.hpp" #include "HALAL/HALAL.hpp" +#include +#include +#include +#include +#include +#include +#include + #ifdef STLIB_ETH #include "StateMachine/StateOrder.hpp" #endif @@ -17,333 +22,552 @@ using ms = std::chrono::milliseconds; using us = std::chrono::microseconds; using s = std::chrono::seconds; -enum AlarmType { LOW_PRECISION, MID_PRECISION, HIGH_PRECISION }; +template +using FixedVector = StaticVector; + +template +concept IsEnum = std::is_enum_v; + +template +concept ValidTime = std::same_as || std::same_as; + +using Callback = void (*)(); +using Guard = bool (*)(); + + +static constexpr size_t NUMBER_OF_ACTIONS = 20; + +enum AlarmType { Milliseconds = 0, Microseconds = 1 }; class TimedAction { public: - function action; - uint32_t period; - AlarmType alarm_precision; - uint8_t id = -1; + Callback action = nullptr; + uint32_t period = 0; + AlarmType alarm_precision = Milliseconds; + uint8_t id = 255; bool is_on = false; TimedAction() = default; + constexpr bool operator==(const TimedAction&) const = default; }; -class State { -public: - vector cyclic_actions; - vector> on_enter_actions = {}; - vector> on_exit_actions = {}; - vector state_orders_ids = {}; - - void enter(); - void exit(); - template - TimedAction *add_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type); - template - TimedAction * - register_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type); - void unregister_all_timed_actions(); - void register_all_timed_actions(); - void unregister_timed_action(TimedAction *timed_action); - void erase_timed_action(TimedAction *timed_action); - void add_state_order(uint16_t id); +template +struct Transition { + StateEnum target; + Guard predicate; + constexpr bool operator==(const Transition&) const = default; }; -template -TimedAction * -State::register_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type) { - TimedAction timed_action = {}; - timed_action.alarm_precision = precision_type; - timed_action.action = action; - uint32_t miliseconds = - chrono::duration_cast(period).count(); - uint32_t microseconds = - chrono::duration_cast(period).count(); - switch (precision_type) { - case LOW_PRECISION: - timed_action.period = miliseconds; - timed_action.id = Time::register_low_precision_alarm(miliseconds, action); - break; - case MID_PRECISION: - timed_action.period = microseconds; - timed_action.id = Time::register_mid_precision_alarm(microseconds, action); - break; - case HIGH_PRECISION: - timed_action.period = microseconds; - timed_action.id = Time::register_high_precision_alarm(microseconds, action); - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", - precision_type); - break; +template +concept are_transitions = (std::same_as> && ...); + + +template +class State { +private: + FixedVector cyclic_actions = {}; + FixedVector on_enter_actions = {}; + FixedVector on_exit_actions = {}; + StateEnum state = {}; + FixedVector, NTransitions> transitions = {}; + + public: + [[no_unique_address]]FixedVector state_orders_ids = {}; + static constexpr size_t transition_count = NTransitions; + + template + requires are_transitions + consteval State(StateEnum state, T... transitions) + : state(state) + { + (this->transitions.push_back(transitions), ...); + } + + consteval State() = default; + + template + consteval State(const State& other){ + state = other.get_state(); + for(const auto& t : other.get_transitions()) transitions.push_back(t); + for(const auto& a : other.get_cyclic_actions()) cyclic_actions.push_back(a); + for(const auto& a : other.get_enter_actions()) on_enter_actions.push_back(a); + for(const auto& a : other.get_exit_actions()) on_exit_actions.push_back(a); + #ifdef STLIB_ETH + for(const auto& id : other.state_orders_ids) state_orders_ids.push_back(id); + #endif } - timed_action.is_on = true; - cyclic_actions.push_back(timed_action); - return &cyclic_actions.back(); -} - -template -TimedAction * -State::add_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type) { - TimedAction timed_action = {}; - timed_action.alarm_precision = precision_type; - timed_action.action = action; - uint32_t miliseconds = - chrono::duration_cast(period).count(); - uint32_t microseconds = - chrono::duration_cast(period).count(); - switch (precision_type) { - case LOW_PRECISION: - timed_action.period = miliseconds; - break; - case MID_PRECISION: - timed_action.period = microseconds; - break; - case HIGH_PRECISION: - timed_action.period = microseconds; - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", - precision_type); - return nullptr; - break; + + constexpr bool operator==(const State&) const = default; + + + + constexpr const StateEnum& get_state() const { return state; }; + constexpr const auto& get_transitions() const { return transitions; }; + constexpr const auto& get_cyclic_actions() const {return cyclic_actions;}; + constexpr const auto& get_enter_actions() const {return on_enter_actions;}; + constexpr const auto& get_exit_actions() const {return on_exit_actions;}; + + + consteval void add_enter_action(Callback action) + { + on_enter_actions.push_back(action); } - cyclic_actions.push_back(timed_action); - return &cyclic_actions.back(); -} -class StateMachine { + consteval void add_exit_action(Callback action) + { + on_exit_actions.push_back(action); + } + + void enter() + { + for (const auto& action : on_enter_actions) + { + if(action) action(); + } + register_all_timed_actions(); + } + + void exit() + { + unregister_all_timed_actions(); + for (const auto& action : on_exit_actions) + { + if(action) action(); + } + } + + void unregister_all_timed_actions() + { + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& timed_action = cyclic_actions[i]; + if(timed_action.action == nullptr){continue;} + if(!timed_action.is_on){ continue;} + + Scheduler::unregister_task(timed_action.id); + timed_action.is_on = false; + } + } + + void register_all_timed_actions() + { + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& timed_action = cyclic_actions[i]; + if(timed_action.action == nullptr){ continue;} + + switch(timed_action.alarm_precision) + { + case Milliseconds: + timed_action.id = Scheduler::register_task((timed_action.period * 1000), timed_action.action); + break; + case Microseconds: + timed_action.id = Scheduler::register_task(timed_action.period, timed_action.action); + break; + + default: + ErrorHandler("Invalid Alarm Precision"); + return; + break; + } + timed_action.is_on = true; + } + } + + void remove_cyclic_action(TimedAction *timed_action) + { + if(timed_action->is_on){ unregister_timed_action(timed_action);} + + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& slot = cyclic_actions[i]; + if(&slot == timed_action) + { + slot = TimedAction{}; + return; + } + } + } + + void unregister_timed_action(TimedAction* timed_action) + { + Scheduler::unregister_task(timed_action->id); + timed_action->is_on = false; + } + + constexpr void add_state_order(uint16_t id) + { + state_orders_ids.push_back(id); + } + + template + consteval TimedAction * + add_cyclic_action(Callback action, + TimeUnit period){ + TimedAction timed_action = {}; + if constexpr (std::is_same_v){ + timed_action.alarm_precision = Milliseconds; + timed_action.period = period.count(); + } + + else if constexpr (std::is_same_v){ + timed_action.alarm_precision = Microseconds; + timed_action.period = period.count(); + } + + else { + ErrorHandler("Invalid Time Unit"); + } + + timed_action.action = action; + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; + } + + +}; + + +template +struct is_state : std::false_type {}; + +template +struct is_state, StateEnum> : std::true_type {}; + +template +concept IsState = is_state::value; + +template +concept are_states = (IsState && ...); + +/// Interface for State Machines to allow other classes to interact with the state machine without knowing its implementation +class IStateMachine { + public: + virtual constexpr ~IStateMachine() = default; + virtual void check_transitions() = 0; + virtual void set_on(bool is_on) = 0; + virtual void force_change_state(size_t state) = 0; + virtual size_t get_current_state_id() const = 0; + constexpr bool operator==(const IStateMachine&) const = default; +}; + +template +class StateMachine : public IStateMachine { +private: + struct NestedPair + { + StateEnum state; + IStateMachine* machine; + constexpr bool operator==(const NestedPair&) const = default; + }; + public: - typedef uint8_t state_id; + StateEnum current_state; + + constexpr ~StateMachine() override = default; + + void force_change_state(size_t state) override + { + StateEnum new_state = static_cast(state); + if(current_state == new_state) + { + return; + } + #ifdef STLIB_ETH + remove_state_orders(); + #endif + exit(); + current_state = new_state; + enter(); + #ifdef STLIB_ETH + refresh_state_orders(); + #endif + } - state_id initial_state = 0; - state_id current_state = 0; + size_t get_current_state_id() const override + { + return static_cast(current_state); + } bool is_on = true; + void set_on(bool is_on) override + { + this->is_on = is_on; + } - StateMachine(); - StateMachine(state_id initial_state); - - void add_state(state_id state); - void add_transition(state_id old_state, state_id new_state, - function transition); - - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period, - vector states); - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period); - - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period, - vector states); - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period); - - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period); - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period, - vector states); - - void remove_cyclic_action(TimedAction *timed_action); - void remove_cyclic_action(TimedAction *timed_action, state_id state); - - void add_enter_action(function action); - void add_enter_action(function action, state_id state); - - void add_exit_action(function action); - void add_exit_action(function action, state_id state); - - void force_change_state(state_id new_state); - void check_transitions(); - - void add_state_machine(StateMachine &state_machine, state_id state); - - void refresh_state_orders(); - - unordered_map &get_states(); +private: + FixedVector, NStates> states; + FixedVector, NTransitions> transitions = {}; + std::array, NStates> transitions_assoc ={}; + FixedVector nested_state_machine = {}; + + constexpr bool operator==(const StateMachine&) const = default; + + template + consteval void process_state(const State& state, size_t offset) + { + for (const auto& t : state.get_transitions()) + { + transitions.push_back(t); + offset++; + } + + transitions_assoc[static_cast(state.get_state())] = { + offset - state.get_transitions().size(), + state.get_transitions().size()}; + } -#ifdef SIM_ON - uint8_t get_id_in_shm(); -#endif + inline void enter() + { + auto& state = states[static_cast(current_state)]; + state.enter(); + + } -private: - unordered_map states; - unordered_map>> - transitions; - unordered_map nested_state_machine; - - void enter_state(state_id new_state); - void exit_state(state_id old_state); - void register_all_timed_actions(state_id state); - void unregister_all_timed_actions(state_id state); -#ifdef SIM_ON - uint8_t state_machine_id_in_shm; -#endif -}; + inline void exit() + { + auto& state = states[static_cast(current_state)]; + state.exit(); + } + +public: -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); + template ... S> + consteval StateMachine(StateEnum initial_state, S... states) + : current_state(initial_state) + { + //Sort states by their enum value + using StateType = State; + std::array sorted_states; + size_t index = 0; + ((sorted_states[index++] = StateType(states)), ...); + + for(size_t i = 0; i < sorted_states.size(); i++){ + for(size_t j = 0; j < sorted_states.size() - 1; j++){ + if(sorted_states[j].get_state() > sorted_states[j+1].get_state()){ + auto temp = sorted_states[j]; + sorted_states[j] = sorted_states[j+1]; + sorted_states[j+1] = temp; + } + } + } + + //Check that states are contiguous and start from 0 + for(size_t i = 0; i < sorted_states.size(); i++) { + if(static_cast(sorted_states[i].get_state()) != i){ + ErrorHandler("States Enum must be contiguous and start from 0"); + } + } + + for(size_t i = 0; i < sorted_states.size(); i++) { + this->states.push_back(sorted_states[i]); + } + + size_t offset = 0; + for(const auto& s : sorted_states) { + process_state(s, offset); + offset += s.get_transitions().size(); + } + } + + + + void check_transitions() override + { + auto& [i, n] = transitions_assoc[static_cast(current_state)]; + for (auto index = i; index < i + n; ++index) + { + const auto& t = transitions[index]; + if (t.predicate()) + { + exit(); + #ifdef STLIB_ETH + remove_state_orders(); + #endif + current_state = t.target; + enter(); + #ifdef STLIB_ETH + refresh_state_orders(); + #endif + break; + } + } + + for(auto& nested : nested_state_machine) + { + if(nested.state == current_state){ + nested.machine->check_transitions(); + break; + } + } + } + + void start() + { + enter(); + } + + + template + void force_change_state(const State& state) + { + StateEnum new_state = state.get_state(); + if(current_state == new_state) + { + return; + } + + exit(); + #ifdef STLIB_ETH + remove_state_orders(); + #endif + current_state = new_state; + enter(); + #ifdef STLIB_ETH + refresh_state_orders(); + #endif + } + + + + template + consteval TimedAction * + add_cyclic_action(Callback action, + TimeUnit period,const State& state) + { + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { + return states[i].add_cyclic_action(action, period); + } + } + ErrorHandler("Error: The state is not added to the state machine"); return nullptr; } - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); + template + consteval void remove_cyclic_action(TimedAction *timed_action, const State& state) + { + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { + states[i].remove_cyclic_action(timed_action); + return; + } + } + ErrorHandler("Error: The state is not added to the state machine"); + } - if (microseconds % 1000 != 0) { - ErrorHandler("Low precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; + template + consteval void add_enter_action(Callback action, const State& state) + { + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { + states[i].add_enter_action(action); + return; + } + } + ErrorHandler("Error: The state is not added to the state machine"); } - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - LOW_PRECISION); - else - return states[state].add_new_timed_action(action, period, LOW_PRECISION); -} - -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (state_id state : states) { - timed_action = add_low_precision_cyclic_action(action, period, state); + + template + consteval void add_exit_action(Callback action, const State& state) + { + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { + states[i].add_exit_action(action); + return; + } + } + ErrorHandler("Error: The state is not added to the state machine"); } - return timed_action; -} - -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period) { - return add_low_precision_cyclic_action(action, period, current_state); -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return nullptr; + + template + constexpr void add_state_machine(IStateMachine& state_machine, const State& state) + { + for(auto& nested : nested_state_machine) + { + if(nested.state == state.get_state()) + { + ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", static_cast(state.get_state())); + return; + } + } + nested_state_machine.push_back({state.get_state(), &state_machine}); } - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); - if (microseconds % 50 != 0) { - ErrorHandler("Mid precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; + inline void refresh_state_orders() + { + #ifdef STLIB_ETH + if(states[static_cast(current_state)].state_orders_ids.size() != 0) + { + std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size()); + StateOrder::add_state_orders(ids); + } + #endif } - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - MID_PRECISION); - else - return states[state].add_new_timed_action(action, period, MID_PRECISION); -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (state_id state : states) { - timed_action = add_mid_precision_cyclic_action(action, period, state); + inline void remove_state_orders() + { + #ifdef STLIB_ETH + if(states[static_cast(current_state)].state_orders_ids.size() != 0) + { + std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size()); + StateOrder::remove_state_orders(ids); + } + #endif } - return timed_action; -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period) { - return add_mid_precision_cyclic_action(action, period, current_state); -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return nullptr; + + constexpr auto& get_states() + { + return states; } - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); +#ifdef SIM_ON + uint8_t get_id_in_shm(); + uint8_t state_machine_id_in_shm; +#endif +}; - if (microseconds < 1) { - ErrorHandler("High precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; +/* @brief Helper function to create a State instance + * + * @tparam StateEnum Enum type representing the states + * @tparam Transitions Variadic template parameter pack representing the transitions + * @param state The state enum value + * @param transitions The transitions associated with the state + * @return A State instance initialized with the provided state and transitions +* +*/ + +template + requires are_transitions + consteval auto make_state(StateEnum state, Transitions... transitions) { + constexpr size_t number_of_transitions = sizeof...(transitions); + return State(state,transitions...); } - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - HIGH_PRECISION); - else - return states[state].add_new_timed_action(action, period, HIGH_PRECISION); -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (state_id state : states) { - timed_action = add_high_precision_cyclic_action(action, period, state); + /* @brief Helper function to create a StateMachine instance + * + * @tparam States Variadic template parameter pack representing the states + * @param initial_state The initial state enum value + * @param states The states to be included in the state machine + * @return A StateMachine instance initialized with the provided initial state and states + */ + + template + requires are_states + consteval auto make_state_machine(StateEnum initial_state, States... states) { + constexpr size_t number_of_states = sizeof...(states); + constexpr size_t number_of_transitions = (std::remove_reference_t::transition_count + ... + 0); + + return StateMachine(initial_state, states...); } - return timed_action; -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period) { - return add_high_precision_cyclic_action(action, period, current_state); -} diff --git a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp index d5bfa48fe..99d0a3874 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp @@ -1,6 +1,7 @@ #pragma once #include "HALAL/HALAL.hpp" #include "ErrorHandler/ErrorHandler.hpp" +#include class StateOrder : public Order{ public: @@ -10,30 +11,30 @@ class StateOrder : public Order{ }; static OrderProtocol* informer_socket; static uint16_t state_orders_ids_size; - static vector* state_orders_ids; - static StackOrder<0,uint16_t, vector> add_state_orders_order; - static StackOrder<0,uint16_t, vector> remove_state_orders_order; + static std::span state_orders_ids; + static StackOrder<0,uint16_t, std::span> add_state_orders_order; + static StackOrder<0,uint16_t, std::span> remove_state_orders_order; static void set_socket(OrderProtocol& socket){ informer_socket = &socket; } - static void add_state_orders(vector& new_ids){ + static void add_state_orders(std::span new_ids){ if(informer_socket == nullptr) { ErrorHandler("Informer Socket has not been set"); return; - } - add_state_orders_order.set_pointer(1, &new_ids); + }; + state_orders_ids = new_ids; state_orders_ids_size = new_ids.size(); informer_socket->send_order(add_state_orders_order); } - static void remove_state_orders(vector& old_ids){ + static void remove_state_orders(std::span old_ids){ if(informer_socket == nullptr){ ErrorHandler("Informer Socket has not been set"); return; } - remove_state_orders_order.set_pointer(1, &old_ids); + state_orders_ids = old_ids; state_orders_ids_size = old_ids.size(); informer_socket->send_order(remove_state_orders_order); } diff --git a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp index fc2cf39b9..3b4799be4 100644 --- a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp +++ b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp @@ -2,7 +2,7 @@ #include "Protections/Notification.hpp" -StateMachine* ProtectionManager::general_state_machine = nullptr; +IStateMachine* ProtectionManager::general_state_machine = nullptr; Notification ProtectionManager::fault_notification = { ProtectionManager::fault_id, nullptr}; Notification ProtectionManager::warning_notification = { @@ -37,7 +37,7 @@ void ProtectionManager::set_id(Boards::ID board_id) { ProtectionManager::board_id = board_id; } -void ProtectionManager::link_state_machine(StateMachine& general_state_machine, +void ProtectionManager::link_state_machine(IStateMachine& general_state_machine, state_id fault_id) { ProtectionManager::general_state_machine = &general_state_machine; ProtectionManager::fault_state_id = fault_id; @@ -49,13 +49,13 @@ void ProtectionManager::tcp_to_fault() { } void ProtectionManager::to_fault() { - if (general_state_machine->current_state != fault_state_id) { + if (general_state_machine->get_current_state_id() != fault_state_id) { fault_and_propagate(); } } void ProtectionManager::external_to_fault() { - if (general_state_machine->current_state != fault_state_id) { + if (general_state_machine->get_current_state_id() != fault_state_id) { external_trigger = true; fault_and_propagate(); } diff --git a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp deleted file mode 100644 index fc8770749..000000000 --- a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp +++ /dev/null @@ -1,377 +0,0 @@ -/* - * Created by Alejandro - */ - -#include "StateMachine/StateMachine.hpp" -#include "ErrorHandler/ErrorHandler.hpp" - -#ifdef SIM_ON -#include "HALALMock/Services/SharedMemory/SharedMemory.hpp" -#endif - -void State::enter() { - for (function& action : on_enter_actions) { - action(); - } -#ifdef STLIB_ETH - if(state_orders_ids.size() != 0) StateOrder::add_state_orders(state_orders_ids); -#endif -} - -void State::exit() { - for (function& action : on_exit_actions) { - action(); - } -#ifdef STLIB_ETH - if(state_orders_ids.size() != 0) StateOrder::remove_state_orders(state_orders_ids); -#endif -} - -void State::unregister_timed_action(TimedAction* timed_action){ - switch (timed_action->alarm_precision) { - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action->id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action->id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action->id); - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", timed_action->alarm_precision); - return; - break; - } - timed_action->is_on = false; -} - -void State::erase_timed_action(TimedAction* timed_action){ - if(timed_action->is_on) unregister_timed_action(timed_action); - cyclic_actions.erase(find_if(cyclic_actions.begin(), cyclic_actions.end(), [&](TimedAction& other){ - if(&other == timed_action) return true; - return false; - })); -} - -void State::unregister_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ - if(!timed_action.is_on) continue; - switch(timed_action.alarm_precision){ - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action.id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action.id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action.id); - break; - default: - ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); - return; - break; - } - timed_action.is_on = false; - } -} - -void State::register_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ - switch(timed_action.alarm_precision){ - case LOW_PRECISION: - timed_action.id = Time::register_low_precision_alarm(timed_action.period, timed_action.action); - break; - case MID_PRECISION: - timed_action.id = Time::register_mid_precision_alarm(timed_action.period, timed_action.action); - break; - case HIGH_PRECISION: - timed_action.id = Time::register_high_precision_alarm(timed_action.period, timed_action.action); - break; - default: - ErrorHandler("Cannot register timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); - return; - break; - } - timed_action.is_on = true; - } -} - -void State::add_state_order(uint16_t id){ - state_orders_ids.push_back(id); -} - -#ifdef SIM_ON -StateMachine::StateMachine(){ - state_machine_id_in_shm = ++(*SharedMemory::state_machine_count); - SharedMemory::update_current_state(state_machine_id_in_shm,initial_state); -} -#else -StateMachine::StateMachine(){} -#endif - -/** - * This is a constructor for a StateMachine object that initializes the initial state and creates a - * State object for it. - * - * @param initial_state The initial state parameter is an unsigned 8-bit integer that represents the - * starting state of the state machine. - */ -StateMachine::StateMachine(uint8_t initial_state) : - initial_state(initial_state), current_state(initial_state) { - add_state(initial_state); - enter_state(initial_state); - - #ifdef SIM_ON - state_machine_id_in_shm = ++(*SharedMemory::state_machine_count); - SharedMemory::update_current_state(state_machine_id_in_shm,initial_state); - #endif -} - -/** - * The function adds a state to a state machine and sets it as the initial and current state if it is - * the first state added. - * - * @param state The state to be added to the state machine. - * - * @return The function does not return anything. It has a void return type. - */ -void StateMachine::add_state(uint8_t state) { - if (states.contains(state)) { - ErrorHandler("The state %d is already added", state); - return; - } - - if (states.empty()) { - initial_state = state; - current_state = state; - } - states[state]; -} - -/** - * This function adds an action to be executed when entering the current state of a state machine. - * - * @param action The parameter "action" is a function pointer to a function that takes no arguments and - * returns nothing (void). It represents an action that should be executed when the current state of - * the state machine is entered. - */ -void StateMachine::add_enter_action(function action) { - if (not current_state) { - ErrorHandler("The state machine does not have a current state"); - return; - } - states[current_state].on_enter_actions.push_back(action); -} - -/** - * This function adds an action to be executed when entering a specific state in a state machine. - * - * @param action The action parameter is a function pointer to a function that takes no arguments and - * returns nothing. This function will be executed when the state machine transitions to the state - * specified by the state parameter. - * @param state The state parameter is an unsigned 8-bit integer that represents the state to which the - * enter action is being added. - */ -void StateMachine::add_enter_action(function action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return; - } - states[state].on_enter_actions.push_back(action); -} - -/** - * This function adds an exit action to the current state of a state machine. - * - * @param action `action` is a function pointer to a function that takes no arguments and returns - * nothing (`void`). It represents an action that should be executed when the state machine exits the - * current state and transitions to a new state. The `add_exit_action` function adds this action to the - * list of exit actions for - */ -void StateMachine::add_exit_action(function action) { - if (not current_state) { - ErrorHandler("The state machine does not have a current state"); - return; - } - states[current_state].on_exit_actions.push_back(action); -} - -/** - * This function adds an exit action to a specific state in a state machine. - * - * @param action The action parameter is a function pointer to a function that takes no arguments and - * returns nothing. This function represents the action that will be executed when the state machine - * exits the specified state. - * @param state The state parameter is an unsigned 8-bit integer that represents the state to which the - * exit action is being added. - */ -void StateMachine::add_exit_action(function action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine"); - return; - } - states[state].on_exit_actions.push_back(action); -} - -/** - * This function adds a transition between two states in a state machine. - * - * @param old_state The current state of the state machine before the transition occurs. It is - * represented as an unsigned 8-bit integer (uint8_t). - * @param new_state The new state to transition to. It is of type uint8_t. - * @param transition The parameter "transition" is a function pointer to a boolean function that takes - * no arguments. This function represents the condition that must be met in order for the state machine - * to transition from the old_state to the new_state. If the condition is true, the transition will - * occur. If the condition is false - */ -void StateMachine::add_transition(uint8_t old_state, uint8_t new_state, - function transition) { - if (not states.contains(old_state)) { - ErrorHandler("The state %d is not added to the state machine", old_state); - } - - if(not states.contains(new_state)) { - ErrorHandler("The state %d is not added to the state machine", new_state); - return; - } - - transitions[old_state][new_state] = transition; -} - -/** - * The function adds a nested state machine to the current state machine and clears any timed actions - * associated with the nested state machine's current state if it is not the current state. - * - * @param state_machine A reference to a StateMachine object that is being added as a nested state - * machine to the current StateMachine object. - * @param state The state to which the nested state machine is being added. - */ -void StateMachine::add_state_machine(StateMachine& state_machine, uint8_t state) { - if(nested_state_machine.contains(state)){ - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); - return; - } - - if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ - ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); - } - - nested_state_machine[state] = &state_machine; - - if (current_state != state) { - state_machine.is_on = false; - } -} - -/** - * The function checks for state transitions and changes the current state if necessary, and also - * checks for transitions in a nested state machine if applicable. - */ -void StateMachine::check_transitions() { - for (auto const& state_transition : transitions[current_state]) { - if (state_transition.second()) { - force_change_state(state_transition.first); - } - } - - if (nested_state_machine.contains(current_state)) { - nested_state_machine[current_state]->check_transitions(); - } -} - -void StateMachine::force_change_state(uint8_t new_state) { - if (not states.contains(new_state)) { - ErrorHandler("The state %d is not added to the state machine", new_state); - return; - } - - if(current_state == new_state) return; - - unregister_all_timed_actions(current_state); - exit_state(current_state); - - current_state = new_state; - - enter_state(current_state); - register_all_timed_actions(current_state); - - #ifdef SIM_ON - SharedMemory::update_current_state(state_machine_id_in_shm,current_state); - #endif -} - -void StateMachine::remove_cyclic_action(TimedAction* action) { - if (not states.contains(current_state)) { - ErrorHandler("The state %d is not added to the state machine", current_state); - return; - } - - states[current_state].erase_timed_action(action); -} - -void StateMachine::remove_cyclic_action(TimedAction* action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return; - } - - states[state].erase_timed_action(action); -} - - - -void StateMachine::enter_state(state_id state) { - states[state].enter(); - - if (nested_state_machine.contains(state)) { - StateMachine* nested_sm = nested_state_machine[state]; - nested_sm->is_on = true; - nested_sm->enter_state(nested_sm->current_state); - } -} - -void StateMachine::exit_state(state_id state) { - states[state].exit(); - - if (nested_state_machine.contains(state)) { - StateMachine* nested_sm = nested_state_machine[state]; - nested_sm->is_on = false; - nested_sm->exit_state(nested_sm->current_state); - nested_sm->current_state = nested_sm->initial_state; - } -} - -void StateMachine::register_all_timed_actions(state_id state) { - states[state].register_all_timed_actions(); - - if (nested_state_machine.contains(current_state)) { - StateMachine* nested_sm = nested_state_machine[current_state]; - nested_sm->states[nested_sm->current_state].register_all_timed_actions(); - } -} - -void StateMachine::unregister_all_timed_actions(state_id state) { - states[state].unregister_all_timed_actions(); - - if (nested_state_machine.contains(current_state)) { - StateMachine* nested_sm = nested_state_machine[current_state]; - nested_sm->states[nested_sm->current_state].unregister_all_timed_actions(); - } -} - -unordered_map& StateMachine::get_states(){ - return states; -} - -void StateMachine::refresh_state_orders(){ -#ifdef STLIB_ETH - if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); -#endif -} - -#ifdef SIM_ON -uint8_t StateMachine::get_id_in_shm(){ - return state_machine_id_in_shm; -} -#endif \ No newline at end of file diff --git a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp index 38fb3d6bb..1f8449d27 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp @@ -1,7 +1,8 @@ #include "StateMachine/StateOrder.hpp" +#include OrderProtocol* StateOrder::informer_socket = nullptr; uint16_t StateOrder::state_orders_ids_size = 0; -vector* StateOrder::state_orders_ids = nullptr; -StackOrder<0,uint16_t, vector> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, StateOrder::state_orders_ids); -StackOrder<0,uint16_t, vector> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, StateOrder::state_orders_ids); +std::span StateOrder::state_orders_ids; +StackOrder<0,uint16_t, std::span> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids); +StackOrder<0,uint16_t, std::span> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids);