Removed libs

src/manager.cpp

@@ -0,0 +1,105 @@
+/*
+ * manager.cpp
+ *
+ *  Created on: Sep 19, 2023
+ *      Author: Dario
+ */
+
+#include <sta/tacos/manager.hpp>
+#include <sta/tacos/statemachine.hpp>
+#include <sta/debug/debug.hpp>
+
+#include <algorithm>
+
+#include <FreeRTOS.h>
+
+
+namespace sta
+{
+	namespace tacos
+	{
+		void Manager::registerThread(std::shared_ptr<TacosThread> thread, std::list<uint16_t> states)
+		{
+			for (uint16_t state : states)
+			{
+				STA_ASSERT(state < STA_TACOS_NUM_STATES);
+
+				threads_[state].emplace(thread);
+			}
+		}
+
+		void Manager::startThreads(uint16_t state)
+		{
+			STA_ASSERT(state < STA_TACOS_NUM_STATES);
+
+			for (std::shared_ptr<TacosThread> thread : threads_[state])
+			{
+				if (!thread->isRunning())
+				{
+					thread->start();
+				}
+			}
+		}
+
+		void Manager::stopThreads(uint16_t state)
+		{
+			std::set<std::shared_ptr<TacosThread>> terminated;
+
+			for (uint16_t other = 0; other < STA_TACOS_NUM_STATES; ++other)
+			{
+				if (other == state)
+				{
+					continue;
+				}
+
+				for (std::shared_ptr<TacosThread> thread : threads_[other])
+				{
+					// If the thread is currently running but not part of the set of threads that should be running...
+					if (thread->isRunning() && terminated.count(thread) == 0 && threads_[state].count(thread) == 0)
+					{
+						// ...politely request termination.
+						thread->requestTermination();
+						terminated.emplace(thread);
+					}
+				}
+			}
+		}
+
+		void Manager::updateThreads()
+		{
+			uint16_t state = Statemachine::instance()->getCurrentState();
+
+			stopThreads(state);
+			startThreads(state);
+		}
+
+		void Manager::init()
+		{
+			startThreads(Statemachine::instance()->getCurrentState());
+		}
+
+		void Manager::func()
+		{
+			Statemachine::stateChangeEvent.wait(EventFlags::ALL, osWaitForever);
+
+			HeapStats_t stats;
+			vPortGetHeapStats(&stats);
+
+			// Start all new tasks and stop all the tasks that aren't supposed to be running.
+			updateThreads();
+		}
+
+		Manager::Manager()
+			: TacosThread{"Manager", STA_TACOS_MANAGER_PRIORITY}
+		{
+
+		}
+
+		//Manager::~Manager(){}
+
+		Manager* Manager::_instance = nullptr;
+
+	}  // namespace tacos
+}  // namespace sta
+
+

src/startup.cpp

@@ -0,0 +1,91 @@
+/*
+ * startup.cpp
+ *
+ *  Created on: 22 Sep 2023
+ *      Author: Dario
+ */
+
+
+#include <cmsis_os2.h>
+#include <usart.h>
+#include <sta/rtos/mutex.hpp>
+
+// sta-core-specific imports.
+#include <sta/devices/stm32/bus/uart.hpp>
+#include <sta/debug/printing/printable_uart.hpp>
+#include <sta/debug/debug.hpp>
+#include <sta/lang.hpp>
+
+// Tacos-specific includes.
+#include <sta/tacos/manager.hpp>
+#include <sta/tacos/statemachine.hpp>
+#include <sta/tacos/startup.hpp>
+
+
+// The UART mutex defined in freertos.c
+extern osMutexId_t uartMutexHandle;
+
+
+namespace sta
+{
+	// Here the printable used for debugging is defined.
+	Printable * Debug;
+
+	namespace tacos
+	{
+		void initPrintable()
+		{
+			// Initialize the mutex for UART communication.
+			RtosMutex * mutex = new RtosMutex(&uartMutexHandle);
+
+			// Initialize the UART interface and printable object.
+			UARTSettings settings = { .mode =  UARTMode::RX_TX };
+			STM32UART * intf_ptr = new STM32UART(&huart2, settings, mutex);
+			Debug = new PrintableUART(intf_ptr);
+
+			STA_DEBUG_PRINTLN("UART SUCCESSFULLY INITIALIZED");
+		}
+
+
+		STA_WEAK
+		void onStatemachineInit()
+		{}
+
+
+		void initStatemachine()
+		{
+			onStatemachineInit();
+
+			Statemachine::instance()->start();
+		}
+
+
+		STA_WEAK
+		void onManagerInit()
+		{}
+
+
+		void initManager()
+		{
+			onManagerInit();
+
+			Manager::instance()->start();
+		}
+	}  // namespace tacos
+
+
+	namespace rtos
+	{
+		// Override the weak implementation of startupExtras provided in rtos2-utils.
+		void startupExtras(void * argument)
+		{
+			tacos::initPrintable();
+
+			tacos::initStatemachine();
+
+			tacos::initManager();
+		}
+	} // namespace rtos
+} // namespace sta
+
+

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

src/thread.cpp

@@ -0,0 +1,127 @@
+/*
+ * thread.cpp
+ *
+ *  Created on: Sep 14, 2023
+ *      Author: Dario
+ */
+
+
+#include <sta/tacos/thread.hpp>
+#include <sta/debug/assert.hpp>
+#include <sta/debug/debug.hpp>
+#include <sta/rtos/system/events.hpp>
+
+#include <functional>
+#include <cstring>
+
+
+namespace sta
+{
+	namespace tacos
+	{
+		TacosThread::TacosThread(const char* name, osPriority_t prio)
+				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				  instance_{ NULL },
+				  attribs_{ .name = name, .priority = prio },
+				  running_{false}
+		{}
+
+		TacosThread::TacosThread()
+				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				  instance_{ NULL },
+				  attribs_{ },
+				  running_{false}
+		{}
+
+		void TacosThread::entry_point(void* arg)
+		{
+			STA_ASSERT(arg != nullptr);
+
+			TacosThread* instance = reinterpret_cast<TacosThread*>(arg) ;
+			instance->loop();
+		}
+
+		void TacosThread::start()
+		{
+			STA_ASSERT(!isRunning());
+
+			// If this is the first time starting the thread, it has to be started via rtos first.
+			if (instance_ == NULL)
+			{
+				instance_ = osThreadNew(entry_point, this, &attribs_);
+
+				STA_ASSERT(instance_ != NULL);
+			}
+
+			// Send a thread start signal.
+			sysNotify(STA_RTOS_THREAD_FLAG_START);
+		}
+
+		bool TacosThread::isRunning()
+		{
+			return running_;
+		}
+
+		osThreadId_t TacosThread::getInstance()
+		{
+			STA_ASSERT(isRunning());
+
+			return instance_;
+		}
+
+		const char* TacosThread::getName() const
+		{
+			return attribs_.name;
+		}
+
+		void TacosThread::init() {}
+
+		void TacosThread::loop()
+		{
+			// Infinite loop allowing quick restarts of this thread after termination.
+			while (true)
+			{
+				// The thread has to wait until the system startup has been completed.
+				rtos::waitForStartupEvent();
+
+				// Wait for a thread start flag.
+				wait(STA_RTOS_THREAD_FLAG_START);
+
+				// Call user-defined initialization code.
+				running_ = true;
+				init();
+
+				// Run the thread until the termination flag is set. Reset this
+				while (!isTerminationRequested())
+				{
+					func();
+				}
+
+				// Clear the termination request flag for this thread.
+				deleteTerminationRequest();
+
+				// Call user-defined cleanup code.
+				cleanup();
+				running_ = false;
+			}
+
+			osThreadExit();
+		}
+
+		void TacosThread::cleanup() {}
+
+		bool TacosThread::operator==(const TacosThread& other) const
+		{
+			return std::strcmp(this->getName(), other.getName()) == 0;
+		}
+
+		bool TacosThread::operator<(const TacosThread& other) const
+		{
+			return std::strcmp(this->getName(), other.getName()) < 0;
+		}
+
+		TacosThread::~TacosThread(){}
+
+	}  // namespace tacos
+}  // namespace sta
+