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

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();
 
 			/**