Added thread rework, and cleaned up Tacos startup

Tacos/src/manager.cpp

@@ -11,6 +11,8 @@
 
 #include <algorithm>
 
+#include <FreeRTOS.h>
+
 
 namespace sta
 {
@@ -41,6 +43,8 @@ namespace sta
 
 		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)
@@ -51,13 +55,11 @@ namespace sta
 				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() && threads_[state].count(thread) == 0)
+					if (thread->isRunning() && terminated.count(thread) == 0 && threads_[state].count(thread) == 0)
 					{
 						// ...politely request termination.
 						thread->requestTermination();
-
-						STA_DEBUG_PRINT("KILLING ");
-						STA_DEBUG_PRINTLN(thread->getName());
+						terminated.emplace(thread);
 					}
 				}
 			}
@@ -67,8 +69,8 @@ namespace sta
 		{
 			uint16_t state = Statemachine::instance()->getCurrentState();
 
-			startThreads(state);
 			stopThreads(state);
+			startThreads(state);
 		}
 
 		void Manager::init()
@@ -80,8 +82,6 @@ namespace sta
 		{
 			Statemachine::stateChangeEvent.wait(Statemachine::EventFlags::ALL, osWaitForever);
 
-			STA_DEBUG_PRINTLN("UPDATING THREADS");
-
 			// Start all new tasks and stop all the tasks that aren't supposed to be running.
 			updateThreads();
 		}

Tacos/src/statemachine.cpp

@@ -17,6 +17,7 @@ namespace sta
 		Statemachine::Statemachine()
 			: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY},
 			  currentState_{STA_TACOS_INITIAL_STATE},
+			  failsafeState_{STA_TACOS_INITIAL_STATE},
 			  lockoutTimer_{[](void *){}, nullptr},
 			  failsafeTimer_{[](void *){}, nullptr},
 			  transitionFunc_{[](uint16_t) -> uint16_t { return Statemachine::instance()->getCurrentState(); }},
@@ -39,15 +40,21 @@ namespace sta
 			// Check if a state change is desired. Block if the lockoutTimer is still running.
 			if (next != currentState_ && !lockoutTimer_.isRunning())
 			{
+				if (failsafeTimer_.isRunning())
+				{
+					failsafeTimer_.stop();
+				}
+
 				// Call user space code to set the timers again.
 				timerFunc_(next, currentState_, EventFlags::NORMAL);
 
 				// Update the state and trigger the global state changed event.
 				currentState_ = next;
 				Statemachine::stateChangeEvent.set(EventFlags::NORMAL);
+				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
 			}
 
-			Statemachine::stateChangeEvent.clear(EventFlags::ALL);
+			osThreadYield();
 		}
 
 		uint16_t Statemachine::getCurrentState() const
@@ -74,16 +81,16 @@ namespace sta
 			STA_ASSERT(nextState < STA_TACOS_NUM_STATES);
 			STA_ASSERT(!failsafeTimer_.isRunning());
 
-			failsafeTimer_.setCallback([nextState](void *) {
+			failsafeTimer_.setCallback([nextState](void* arg) {
 				Statemachine::instance()->forceStateTransition(nextState);
-			}, nullptr);
+			}, &failsafeState_);
 
 			failsafeTimer_.start(millis);
 		}
 
 		void Statemachine::setLockoutTimer(uint32_t millis)
 		{
-			STA_ASSERT(!failsafeTimer_.isRunning());
+			STA_ASSERT(!lockoutTimer_.isRunning());
 
 			lockoutTimer_.setCallback([](void *) {  }, nullptr);
 			lockoutTimer_.start(millis);
@@ -91,6 +98,17 @@ namespace sta
 
 		void Statemachine::forceStateTransition(uint16_t state)
 		{
+			if (failsafeTimer_.isRunning())
+			{
+				failsafeTimer_.stop();
+			}
+
+			if (lockoutTimer_.isRunning())
+			{
+				lockoutTimer_.stop();
+			}
+
+			timerFunc_(state, currentState_, EventFlags::FORCED);
 			currentState_ = state;
 
 			Statemachine::stateChangeEvent.set(EventFlags::FORCED);

Tacos/src/thread.cpp

@@ -22,13 +22,15 @@ namespace sta
 		TacosThread::TacosThread(const char* name, osPriority_t prio)
 				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
 				  instance_{ NULL },
-				  attribs_{ .name = name, .priority = prio }
+				  attribs_{ .name = name, .priority = prio },
+				  running_{false}
 		{}
 
 		TacosThread::TacosThread()
 				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
 				  instance_{ NULL },
-				  attribs_{ }
+				  attribs_{ },
+				  running_{false}
 		{}
 
 		void TacosThread::entry_point(void* arg)
@@ -43,16 +45,21 @@ namespace sta
 		{
 			STA_ASSERT(!isRunning());
 
-			instance_ = osThreadNew(entry_point, this, &attribs_);
+			// 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);
+				STA_ASSERT(instance_ != NULL);
+			}
 
-			// SetFlag(THREAD_START);
+			// Send a thread start signal.
+			sysNotify(STA_RTOS_THREAD_FLAG_START);
 		}
 
 		bool TacosThread::isRunning()
 		{
-			return instance_ != NULL;
+			return running_;
 		}
 
 		osThreadId_t TacosThread::getInstance()
@@ -67,26 +74,37 @@ namespace sta
 			return attribs_.name;
 		}
 
+		void TacosThread::init() {}
+
 		void TacosThread::loop()
 		{
-			// The thread has to wait until the startup has been completed.
-			rtos::waitForStartupEvent();
-
-			// wait(THREAD_START)
-
-			init();
-
-			// Run the thread until the termination flag is set.
-			while ((getFlags() & STA_RTOS_THREAD_FLAG_TERMINATE) == 0)
+			// Infinite loop allowing quick restarts of this thread after termination.
+			while (true)
 			{
-				func();
-			}
+				// The thread has to wait until the startup has been completed.
+				rtos::waitForStartupEvent();
 
-			instance_ = NULL;
+				// Wait for a thread start flag.
+				wait(STA_RTOS_THREAD_FLAG_START);
+
+				running_ = true;
+				init();
+
+				// Run the thread until the termination flag is set.
+				while (!checkTerminationRequest())
+				{
+					func();
+				}
+
+				cleanup();
+				running_ = false;
+			}
 
 			osThreadExit();
 		}
 
+		void TacosThread::cleanup() {}
+
 		bool TacosThread::operator==(const TacosThread& other) const
 		{
 			return std::strcmp(this->getName(), other.getName()) == 0;