Multiple bugfixes; working manager task

App/Src/startup.cpp

@@ -28,10 +28,10 @@ namespace sta
 		{
 			STA_DEBUG_PRINTLN("Starting manager task!");
 
-			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("A"), {0, 1});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("0"), {0});
-			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("B"), {0, 2});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("1"), {1});
-			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("C"), {0, 1});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("2"), {2});
-			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("D"), {0, 2});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("3"), {3});
 		}
 	} // namespace tacos
 } // namespace sta

Libs/rtos2-utils

@@ -1 +1 @@
-Subproject commit 1028264eefaf0c384bab4391c6151951bd76842e
+Subproject commit d591560c95a4eae525cbbbdb4e4bc15642852473

Tacos/include/sta/tacos/statemachine.hpp

@@ -20,9 +20,14 @@
 #	define STA_TACOS_INITIAL_STATE 0
 #endif
 
+#define TACOS_STATE_TRANSITION_FLAG			0x00000001U
+#define TACOS_STATE_CHANGE_FORCED_FLAG		0x00000002U
+#define TACOS_STATE_CHANGE_ALL_FLAG			0x00000003U
+
 
 #include <sta/tacos/thread.hpp>
 #include <sta/rtos/timer.hpp>
+#include <sta/rtos/event.hpp>
 #include <sta/debug/assert.hpp>
 
 #include <functional>
@@ -35,6 +40,8 @@ namespace sta
 		class Statemachine : public TacosThread
 		{
 		public:
+			static RtosEvent* stateChangeEvent;
+
 			static Statemachine* instance()
 			{
 				static CGuard g;
@@ -43,6 +50,7 @@ namespace sta
 				{
 					// Create a the manager singleton instance.
 					Statemachine::_instance = new Statemachine();
+					Statemachine::stateChangeEvent = new RtosEvent();
 				}
 
 				return _instance;
@@ -52,10 +60,17 @@ namespace sta
 
 			void func() override;
 
+			/**
+			 * @brief Returns the statemachine's current state.
+			 */
 			uint16_t getCurrentState() const;
 
-			void setStateTransitionFunction(uint16_t (*function)(uint16_t));
+			/**
+			 * @brief Registers a new state transition function.
+			 */
+			void setStateTransitionFunction(std::function<uint16_t(uint16_t)> function);
 
+			void forceStateTransition(uint16_t state);
 		private:
 			static Statemachine * _instance;
 
@@ -67,7 +82,9 @@ namespace sta
 				  if( NULL != Statemachine::_instance )
 				  {
 					 delete Statemachine::_instance;
+					 delete Statemachine::stateChangeEvent;
 					 Statemachine::_instance = NULL;
+					 Statemachine::stateChangeEvent = NULL;
 				  }
 			   }
 			};
@@ -77,10 +94,6 @@ namespace sta
 			Statemachine(const Statemachine&);
 
 			~Statemachine() {}
-
-		private:
-			static void forceStateChange(void * arg);
-
 		private:
 			uint16_t currentState_;
 			RtosTimer lockoutTimer_;

Tacos/src/manager.cpp

@@ -9,6 +9,8 @@
 #include <sta/tacos/statemachine.hpp>
 #include <sta/debug/debug.hpp>
 
+#include <algorithm>
+
 
 namespace sta
 {
@@ -39,22 +41,23 @@ namespace sta
 
 		void Manager::stopThreads(uint16_t state)
 		{
-			uint16_t currentState = Statemachine::instance()->getCurrentState();
+			for (uint16_t other = 0; other < STA_TACOS_NUM_STATES; ++other)
-
-			for (uint16_t state = 0; state < STA_TACOS_NUM_STATES; ++state)
 			{
-				if (state == currentState)
+				if (other == state)
 				{
 					continue;
 				}
 
-				for (std::shared_ptr<TacosThread> thread : threads_[state])
+				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_[currentState].count(thread) == 0)
+					if (thread->isRunning() && threads_[state].count(thread) == 0)
 					{
 						// ...politely request termination.
 						thread->requestTermination();
+
+						STA_DEBUG_PRINT("KILLING ");
+						STA_DEBUG_PRINTLN(thread->getName());
 					}
 				}
 			}
@@ -75,14 +78,9 @@ namespace sta
 
 		void Manager::func()
 		{
-			// Wait for either the termination request or the state change flag.
+			Statemachine::stateChangeEvent->wait(TACOS_STATE_CHANGE_ALL_FLAG, osWaitForever);
-			uint32_t flags = osEventFlagsWait(getInstance(), STA_RTOS_THREAD_FLAG_TERMINATE, osFlagsWaitAny, osWaitForever);
 
-			if ((flags & STA_RTOS_THREAD_FLAG_TERMINATE) != 0)
+			STA_DEBUG_PRINTLN("UPDATING THREADS");
-			{
-				// The loop implemented by the TacosThread class should handle termination.
-				return;
-			}
 
 			// Start all new tasks and stop all the tasks that aren't supposed to be running.
 			updateThreads();

Tacos/src/statemachine.cpp

@@ -18,7 +18,8 @@ namespace sta
 			: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY},
 			  currentState_{STA_TACOS_INITIAL_STATE},
 			  lockoutTimer_{[](void *){}, nullptr},
-			  failsafeTimer_{[](void *){}, nullptr}
+			  failsafeTimer_{[](void *){}, nullptr},
+			  transitionFunc_{[](uint16_t) -> uint16_t { return Statemachine::instance()->getCurrentState(); }}
 		{
 			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
 		}
@@ -28,15 +29,14 @@ namespace sta
 			lockoutTimer_.setCallback([](void *) {  }, nullptr);
 			lockoutTimer_.start(5000);
 
-			STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE");
+			failsafeTimer_.setCallback([](void *) {
-
+				Statemachine::instance()->forceStateTransition(1);
-			failsafeTimer_.setCallback([](void * arg) {  }, nullptr);
+			}, nullptr);
 			failsafeTimer_.start(10000);
 		}
 
 		void Statemachine::func()
 		{
-			/*
 			uint16_t next = transitionFunc_(currentState_);
 
 			STA_ASSERT(next < STA_TACOS_NUM_STATES);
@@ -45,10 +45,10 @@ namespace sta
 			{
 				currentState_ = next;
 
-				// TODO: Emit state transition event.
+				Statemachine::stateChangeEvent->set(TACOS_STATE_TRANSITION_FLAG);
-			}*/
+			}
-			STA_DEBUG_PRINTLN(lockoutTimer_.isRunning() ? "[RUNNING]" : "[STOPPED]");
+
-			osDelay(1000);
+			Statemachine::stateChangeEvent->clear(TACOS_STATE_CHANGE_ALL_FLAG);
 		}
 
 		uint16_t Statemachine::getCurrentState() const
@@ -56,14 +56,23 @@ namespace sta
 			return currentState_;
 		}
 
-		void Statemachine::setStateTransitionFunction(uint16_t (*function)(uint16_t))
+		void Statemachine::setStateTransitionFunction(std::function<uint16_t(uint16_t)> function)
 		{
 			STA_ASSERT(function != nullptr);
 
 			transitionFunc_ = function;
 		}
 
+		void Statemachine::forceStateTransition(uint16_t state)
+		{
+			currentState_ = state;
+
+			Statemachine::stateChangeEvent->set(TACOS_STATE_CHANGE_FORCED_FLAG);
+			Statemachine::stateChangeEvent->clear(TACOS_STATE_CHANGE_ALL_FLAG);
+		}
+
 		Statemachine* Statemachine::_instance = nullptr;
+		RtosEvent* Statemachine::stateChangeEvent = nullptr;
 
 	}  // namespace tacos
 }  // namespace sta

Tacos/src/thread.cpp

@@ -9,6 +9,7 @@
 #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>
@@ -66,9 +67,13 @@ namespace sta
 
 		void TacosThread::loop()
 		{
+			// The thread has to wait until the startup has been completed.
+			rtos::waitForStartupEvent();
+
 			init();
 
-			while ((osEventFlagsGet(instance_) & STA_RTOS_THREAD_FLAG_TERMINATE) == 0)
+			// Run the thread until the termination flag is set.
+			while ((getFlags() & STA_RTOS_THREAD_FLAG_TERMINATE) == 0)
 			{
 				func();
 			}