Working manager & statemachine tasks; timers still bugged; statemachine init bugged

App/Src/startup.cpp

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

Libs/rtos2-utils

@@ -1 +1 @@
-Subproject commit 5b8c97cf5c4b57702c25c0f7e7bcaea4575a6f7d
+Subproject commit 2948c9203e44a5126e6a0067522698964f0ee7c4

Libs/sta-core

@@ -1 +1 @@
-Subproject commit 8e93db18174937562f246af4b1e8d99bd6bbf632
+Subproject commit 4dc625f6e9ebe8db0fc795719a26ee900425b49a

Tacos/include/sta/tacos/manager.hpp

@@ -34,9 +34,6 @@ namespace sta
 				{
 					// Create a the manager singleton instance.
 					Manager::_instance = new Manager();
-
-					// Start the manager task as a tacos task.
-					Manager::_instance->start();
 				}
 
 				return _instance;
@@ -48,8 +45,22 @@ namespace sta
 
 			void func() override;
 
-			static Manager* _instance;
 		private:
+			static Manager* _instance;
+
+			class CGuard
+			{
+			public:
+			   ~CGuard()
+			   {
+				  if( NULL != Manager::_instance )
+				  {
+					 delete Manager::_instance;
+					 Manager::_instance = NULL;
+				  }
+			   }
+			};
+
 			Manager();
 
 			Manager(const Manager&);
@@ -63,19 +74,6 @@ namespace sta
 			void stopThreads(uint16_t state);
 
 			std::set<TacosThread> threads_[STA_TACOS_NUM_STATES];
-
-			class CGuard
-						{
-						public:
-						   ~CGuard()
-						   {
-							  if( NULL != Manager::_instance )
-							  {
-								 delete Manager::_instance;
-								 Manager::_instance = NULL;
-							  }
-						   }
-						};
 		};
 	}  // namespace tacos
 }  // namespace sta

Tacos/include/sta/tacos/statemachine.hpp

@@ -11,6 +11,20 @@
 #include <sta/config.hpp>
 
 #include <sta/tacos/thread.hpp>
+#include <sta/rtos/timer.hpp>
+
+#include <sta/debug/assert.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
+
 
 namespace sta
 {
@@ -27,9 +41,6 @@ namespace sta
 				{
 					// Create a the manager singleton instance.
 					Statemachine::_instance = new Statemachine();
-
-					// Start the manager task as a tacos task.
-					Statemachine::_instance->start();
 				}
 
 				return _instance;
@@ -39,7 +50,7 @@ namespace sta
 
 			void func() override;
 
-			uint16_t getCurrentState();
+			uint16_t getCurrentState() const;
 
 		private:
 			static Statemachine * _instance;
@@ -63,11 +74,17 @@ namespace sta
 
 			~Statemachine() {}
 
+		private:
+			static void forceStateChange(void * arg);
+
+		private:
 			uint16_t currentState_;
+			RtosTimer lockoutTimer_;
+			RtosTimer failsafeTimer_;
 		};
 	}  // namespace tacos
 }  // namespace sta
 
-
+#endif // STA_TACOS_NUM_STATES
 
 #endif /* INCLUDE_TACOS_STATEMACHINE_HPP_ */

Tacos/include/sta/tacos/thread.hpp

@@ -58,6 +58,9 @@ namespace sta
 			 */
 			void loop();
 
+			/**
+			 * @brief This function is executed first when this thread is started.
+			 */
 			virtual void init();
 
 			/**

Tacos/src/manager.cpp

@@ -7,6 +7,7 @@
 
 #include <sta/tacos/manager.hpp>
 #include <sta/tacos/statemachine.hpp>
+#include <sta/debug/debug.hpp>
 
 
 namespace sta
@@ -69,11 +70,20 @@ namespace sta
 
 		void Manager::init()
 		{
+			STA_DEBUG_PRINTLN("INITIALIZING MANAGER!");
+
 			startThreads(Statemachine::instance()->getCurrentState());
 		}
 
 		void Manager::func()
 		{
+			if (true)
+			{
+				// STA_DEBUG_PRINTLN("LOOPY LOOP IN MANAGER TASK");
+				osDelay(1000);
+			}
+
+
 			// Wait for either the termination request or the state change flag.
 			uint32_t flags = osEventFlagsWait(getInstance(), STA_RTOS_THREAD_FLAG_TERMINATE, osFlagsWaitAny, osWaitForever);
 

Tacos/src/startup.cpp

@@ -69,7 +69,7 @@ namespace sta
 		{
 			onManagerInit();
 
-			Statemachine::instance()->start();
+			Manager::instance()->start();
 		}
 	}  // namespace tacos
 

Tacos/src/statemachine.cpp

@@ -7,23 +7,43 @@
 
 #include <sta/tacos/statemachine.hpp>
 
+#include <sta/debug/debug.hpp>
+
+
 namespace sta
 {
 	namespace tacos
 	{
-		Statemachine::Statemachine(){}
+		Statemachine::Statemachine()
+			: currentState_{STA_TACOS_INITIAL_STATE}, lockoutTimer_{[](void *){}, NULL}, failsafeTimer_{[](void *){}, NULL}
+		{
+			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
+		}
 
 		void Statemachine::init()
 		{
+			STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE");
 
+			/*
+			lockoutTimer_.setCallback([](void *) { STA_DEBUG_PRINTLN("Lockout timer triggered!"); }, NULL);
+			lockoutTimer_.start(1000);
+
+			failsafeTimer_.setCallback([](void *) { STA_DEBUG_PRINTLN("Failsafe timer triggered!"); }, NULL);
+			failsafeTimer_.start(2000);
+			*/
 		}
 
 		void Statemachine::func()
 		{
+			// STA_DEBUG_PRINTLN("LOOPY LOOP IN STATEMACHINE");
 
+			STA_DEBUG_PRINT("Failsafe timer is running: ");
+			STA_DEBUG_PRINTLN(failsafeTimer_.isRunning());
+
+			osDelay(1000);
 		}
 
-		uint16_t Statemachine::getCurrentState()
+		uint16_t Statemachine::getCurrentState() const
 		{
 			return currentState_;
 		}

Tacos/src/thread.cpp

@@ -19,11 +19,14 @@ namespace sta
 	{
 		TacosThread::TacosThread(const char* name, osPriority_t prio)
 				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				  instance_{ NULL },
 				  attribs_{ .name = name, .priority = prio }
 		{}
 
 		TacosThread::TacosThread()
-				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_)))
+				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				  instance_{ NULL },
+				  attribs_{ }
 		{}
 
 		void TacosThread::entry_point(void* arg)
@@ -85,6 +88,7 @@ namespace sta
 		}
 
 		void TacosThread::init(){}
+
 		void TacosThread::func(){}
 
 		TacosThread::~TacosThread(){}