Removed libs

src/statemachine.cpp

@@ -0,0 +1,107 @@
+/*
+ * 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},
+			  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 */)
+		{
+			StateTransition transition;
+			transition.from = from;
+			transition.to = to;
+			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);
+		}
+
+		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
+		{
+			STA_ASSERT(to < STA_TACOS_NUM_STATES);
+			STA_ASSERT(!failsafeTimer_.isRunning());
+
+			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
+
+
+