Working CAN sending on TACOS via user space

Tacos/include/sta/tacos/manager.hpp

@@ -0,0 +1,87 @@
+/*
+ * manager.hpp
+ *
+ *  Created on: Sep 19, 2023
+ *      Author: Dario
+ */
+
+#ifndef INCLUDE_STA_TACOS_MANAGER_HPP_
+#define INCLUDE_STA_TACOS_MANAGER_HPP_
+
+
+#include <sta/config.hpp>
+
+#ifndef STA_TACOS_MANAGER_PRIORITY
+#	error "Manger task priority not specified in config.hpp"
+#else
+
+#include <set>
+#include <list>
+#include <memory>
+#include <sta/tacos/thread.hpp>
+
+namespace sta
+{
+	namespace tacos
+	{
+		class Manager : public TacosThread
+		{
+		public:
+			static Manager* instance()
+			{
+				static CGuard g;
+
+				if (!_instance)
+				{
+					// Create a the manager singleton instance.
+					Manager::_instance = new Manager();
+				}
+
+				return _instance;
+			}
+
+			/**
+			 *
+			 */
+			void registerThread(std::shared_ptr<TacosThread> thread, std::list<uint16_t> states);
+
+			void init() override;
+
+			void func() override;
+
+		private:
+			static Manager* _instance;
+
+			class CGuard
+			{
+			public:
+			   ~CGuard()
+			   {
+				  if( NULL != Manager::_instance )
+				  {
+					 delete Manager::_instance;
+					 Manager::_instance = NULL;
+				  }
+			   }
+			};
+
+			Manager();
+
+			Manager(const Manager&);
+
+			//~Manager();
+
+			void updateThreads();
+
+			void startThreads(uint16_t state);
+
+			void stopThreads(uint16_t state);
+
+			std::set<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
+		};
+	}  // namespace tacos
+}  // namespace sta
+
+#endif // STA_TACOS_MANAGER_PRIORITY
+
+#endif /* INCLUDE_STA_TACOS_MANAGER_HPP_ */

Tacos/include/sta/tacos/startup.hpp

@@ -0,0 +1,31 @@
+/*
+ * startup.hpp
+ *
+ *  Created on: 22 Sep 2023
+ *      Author: Dario
+ */
+
+#ifndef INCLUDE_STA_TACOS_STARTUP_HPP_
+#define INCLUDE_STA_TACOS_STARTUP_HPP_
+
+namespace sta
+{
+	namespace tacos
+	{
+		/**
+		 * @brief Function that is called before the statemachine task is started.
+		 * Override it to adjust the statemachine to your specifications.
+		 */
+		void onStatemachineInit();
+
+		/**
+		 * @brief Function that is called before the manager task is started.
+		 * Override it to adjust the manager to your specifications.
+		 */
+		void onManagerInit();
+	}  // namespace tacos
+}  // namespace sta
+
+
+
+#endif /* INCLUDE_STA_TACOS_STARTUP_HPP_ */

Tacos/include/sta/tacos/statemachine.hpp

@@ -0,0 +1,158 @@
+/*
+ * statemachine.hpp
+ *
+ *  Created on: Sep 14, 2023
+ *      Author: Dario
+ */
+
+#ifndef INCLUDE_TACOS_STATEMACHINE_HPP_
+#define INCLUDE_TACOS_STATEMACHINE_HPP_
+
+#include <sta/config.hpp>
+
+
+#ifndef STA_TACOS_NUM_STATES
+#	error "Number of states wasn't defined in config.hpp"
+#else
+
+
+#ifndef STA_TACOS_INITIAL_STATE
+#	define STA_TACOS_INITIAL_STATE 0
+#endif
+
+#define STA_TACOS_STATEMACHINE_QUEUE_LENGTH		4
+
+// State transition happened because of
+#define STA_TACOS_STATE_CHANGE_NORMAL_FLAG		( 0x1U )
+#define STA_TACOS_STATE_CHANGE_FORCED_FLAG		( 0x1U << 1)
+#define STA_TACOS_STATE_CHANGE_TIMEOUT			( 0x1U << 2)
+#define STA_TACOS_STATE_CHANGE_STARTUP_FLAG			( 0x1U << 3)
+#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 <functional>
+#include <tuple>
+
+
+namespace sta
+{
+	namespace tacos
+	{
+		enum EventFlags
+		{
+			NORMAL 				= 0x1U,
+			FORCED				= 0x1U << 1,
+			TIMEOUT				= 0x1U << 2,
+			STARTUP				= 0x1U << 3,
+			ALL					= NORMAL | FORCED | TIMEOUT | STARTUP
+		};
+
+		struct StateTransition
+		{
+			uint16_t from;
+			uint16_t to;
+			EventFlags event;
+			uint32_t lockout;
+		};
+
+
+		class Statemachine : public TacosThread
+		{
+		public:
+			/**
+			 * @brief The global event signaling a state change.
+			 */
+			static RtosEvent stateChangeEvent;
+
+			/**
+			 * @brief Getter function for the singleton instance.
+			 */
+			static Statemachine* instance()
+			{
+				static CGuard g;
+
+				if (!_instance)
+				{
+					// Create the manager singleton instance.
+					Statemachine::_instance = new Statemachine();
+				}
+
+				return _instance;
+			}
+
+			/**
+			 * @brief Returns the statemachine's current state.
+			 */
+			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.
+			 */
+			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0);
+
+			/**
+			 * @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.
+			 */
+			void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
+
+			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.
+			 */
+			void setLockoutTimer(uint32_t millis);
+
+		private:
+			uint16_t currentState_;
+
+			RtosTimer lockoutTimer_;
+			RtosTimer failsafeTimer_;
+
+			RtosQueue<StateTransition> queue_;
+		};
+	}  // namespace tacos
+}  // namespace sta
+
+#endif // STA_TACOS_NUM_STATES
+
+#endif /* INCLUDE_TACOS_STATEMACHINE_HPP_ */

Tacos/include/sta/tacos/thread.hpp

@@ -0,0 +1,90 @@
+/*
+ * task.hpp
+ *
+ *  Created on: Sep 14, 2023
+ *      Author: Dario
+ */
+
+#ifndef INCLUDE_TACOS_TASK_HPP_
+#define INCLUDE_TACOS_TASK_HPP_
+
+#include <cmsis_os2.h>
+
+#include <sta/rtos/thread.hpp>
+
+
+namespace sta
+{
+	namespace tacos
+	{
+		class TacosThread : public RtosThread
+		{
+		public:
+			/**
+			 *
+			 */
+			TacosThread(const char* name, osPriority_t prio);
+
+			TacosThread();
+
+			virtual ~TacosThread();
+
+			/**
+			 * @brief Start the execution of this thread.
+			 */
+			void start();
+
+			/**
+			 * @brief Checks if this thread is currently running.
+			 */
+			bool isRunning();
+
+			/**
+			 * @brief Get the currently running instance.
+			 */
+			osThreadId_t getInstance();
+
+			/**
+			 *
+			 */
+			const char* getName() const;
+
+			bool operator==(const TacosThread& other) const;
+			bool operator<(const TacosThread& other) const;
+
+			/**
+			 * @brief A function that wraps this task's functionality in a loop. This loop will run until
+			 * termination is requested.
+			 */
+			void loop();
+
+			/**
+			 * @brief This function is executed first when this thread is started.
+			 */
+			virtual void init();
+
+			/**
+			 * @brief The body of the thread's loop. Has to be implemented by the user.
+			 */
+			virtual void func() = 0;
+
+
+			virtual void cleanup();
+
+		private:
+
+			/**
+			 * @brief Static function to pass to RTOS to run as a thread. Calls the loop function implemented here.
+			 */
+			static void entry_point(void* arg);
+
+		private:
+			osThreadId_t instance_;
+			osThreadAttr_t attribs_;
+			bool running_;
+		};
+	}
+}
+
+
+#endif /* INCLUDE_TACOS_TASK_HPP_ */

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

Tacos/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(&huart4, 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
+
+

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

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