Added dummy statemachine

App/Src/tasks/manager.cpp

@@ -6,60 +6,117 @@
  */
 
 
+#include <list>
+#include <algorithm>
+
 #include <cmsis_os2.h>
 #include <FreeRTOS.h>
 
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
-
 #include <sta/rtos/system/events.hpp>
 
-typedef StaticTask_t osStaticThreadDef_t;
+
+#define STATE_CHANGED_MSK 0x01
+#define STATE_CHANGED_TIMOUT_MSK 0x02
+
+// The state changed event defined somewhere else.
+extern osEventFlagsId_t stateChangeEvent_id;
+
+// The current state defined somewhere else.
+extern tacos_states_t currentState;
 
 
-extern "C" void outputTask(void *);
-
-
-void startWorker(void *arg)
+// The data structure representing a TACOS task.
+struct tacos_task_t
 {
-	STA_DEBUG_PRINTLN("STARTED WORKER!");
+	// The code to be executed for this task.
+	osThreadFunc_t func;
+
+	// The attributes for the task.
+	osThreadAttr_t attribs;
+
+	// A list of currently running instances of this task.
+	std::list<osThreadId_t> running;
+
+	// A list of states for which this task should be running.
+	std::list<tacos_states_t> states;
+};
+
+std::list<tacos_task_t> tasks;
+
+
+
+void dummyInit(void *)
+{
+	while (true)
+	{
+		STA_DEBUG_PRINTLN("INIT STATE");
+	}
 
 	osThreadExit();
 }
 
+void dummyStated(void *)
+{
+	while (true)
+	{
+		STA_DEBUG_PRINTLN("STARTED STATE");
+	}
+
+	osThreadExit();
+}
+
+void dummyFlying(void *)
+{
+	while (true)
+	{
+		STA_DEBUG_PRINTLN("FLYING STATE");
+	}
+
+	osThreadExit();
+}
+
+void dummyLanded(void *)
+{
+	while (true)
+	{
+		STA_DEBUG_PRINTLN("LANDED STATE");
+	}
+
+	osThreadExit();
+}
+
+
 extern "C" void startManagerTask(void *)
 {
 	STA_DEBUG_PRINTLN("INITIALIZED MANAGER TASK");
 
-	/*
-	// Create thread using static allocation
-	osThreadId_t outputTaskHandle;
-	uint32_t outputTaskBuffer[ 128 ];
-	osStaticThreadDef_t outputTaskControlBlock;
-	const osThreadAttr_t outputTask_attributes = {
-		.name = "outputTask",
-		.cb_mem = &outputTaskControlBlock,
-		.cb_size = sizeof(outputTaskControlBlock),
-		.stack_mem = &outputTaskBuffer[0],
-		.stack_size = sizeof(outputTaskBuffer),
-		.priority = (osPriority_t) osPriorityLow,
-	};
-
-	outputTaskHandle = osThreadNew(outputTask, NULL, &outputTask_attributes);
-	STA_ASSERT_MSG(outputTaskHandle != nullptr, "outputTask initialization failed");
-	*/
-
-	// Create thread using static allocation
-	const osThreadAttr_t workerAttributes = {};
-
-	for (uint8_t i = 0; i < 5; i++)
-	{
-		osThreadNew(startWorker, NULL, &workerAttributes);
-	}
-
 	while (true)
 	{
-		// STA_DEBUG_PRINTLN("MANAGING!");
+		// Wait until the state machine triggers an event signaling a state-change.
+		osEventFlagsWait(stateChangeEvent_id, STATE_CHANGED_MSK, osFlagsWaitAll, osWaitForever);
+
+		for (tacos_task_t task : tasks)
+		{
+			// Check if this task is supposed to be running for this state. If not, kill all instances, else create the desired number of instances.
+			if (std::find(task.states.begin(), task.states.end(), currentState) == task.states.end())
+			{
+				for (osThreadId_t instance : task.running)
+				{
+					osThreadTerminate(instance);
+				}
+			}
+			else
+			{
+				if (!task.running.empty())
+				{
+					osThreadCreate(thread_def, argument);
+				}
+			}
+		}
+
+		STA_DEBUG_PRINTLN("STARTING NEW TASKS DUE TO STATE CHANGE!");
 	}
 
 	osThreadExit();