Added dummy statemachine

App/Inc/tasks/statemachine.hpp

@@ -0,0 +1,32 @@
+/*
+ * statemachine.hpp
+ *
+ *  Created on: Sep 6, 2023
+ *      Author: Dario
+ */
+
+#ifndef INC_TASKS_STATEMACHINE_HPP_
+#define INC_TASKS_STATEMACHINE_HPP_
+
+// Two flags for the state change event. TODO: HOW TO SET THE VALUES?
+#define TACOS_STATE_CHG_FORCED 	0x01
+#define TACOS_STATE_CHG_TIMEOUT	0x02
+#define TACOS_STATE_CHG_NATURAL	0x03
+#define TACOS_STATE_CHG_ALL		TACOS_STATE_CHG_FORCED | TACOS_STATE_CHG_TIMEOUT | TACOS_STATE_CHG_NATURAL
+
+
+// The event for signaling state changes to other tasks
+osEventFlagsId_t stateChangeEvent_id;
+
+
+// The states used for this demo
+enum tacos_states_t
+{
+	init,
+	started,
+	flying,
+	landed
+};
+
+
+#endif /* INC_TASKS_STATEMACHINE_HPP_ */

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 *);
+// The data structure representing a TACOS task.
-
+struct tacos_task_t
-
-void startWorker(void *arg)
 {
-	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();

App/Src/tasks/statemachine.cpp

@@ -0,0 +1,107 @@
+/*
+ * state_machine.cpp
+ *
+ *  Created on: Sep 4, 2023
+ *      Author: Dario
+ */
+
+#include <cmsis_os2.h>
+
+#include <tasks/statemachine.hpp>
+#include <sta/debug/assert.hpp>
+
+
+tacos_states_t currentState;
+
+osTimerId_t lockout;
+osTimerId_t failsafe;
+
+
+void lockoutCallback(void* arg)
+{
+
+}
+
+void failsafeCallback(void* arg)
+{
+	changeState(TACOS_STATE_CHG_TIMEOUT);
+}
+
+
+
+bool checkStateChangeConditions()
+{
+	// Only use the timers in this demo.
+	return false;
+}
+
+
+void changeState(uint8_t flag)
+{
+	// Stop the old timers.
+	osTimerStop(lockout);
+	osTimerStop(failsafe);
+
+	// Set the event flags to signal other tasks that a state change has occured. Reset the flags immediately.
+	osEventFlagsSet(stateChangeEvent_id, flag);
+	osEventFlagsClear(stateChangeEvent_id, TACOS_STATE_CHG_ALL);
+
+	uint8_t lockoutCycles = 0;
+	uint8_t failsafeCycles = 0;
+
+	switch (currentState) {
+		case tacos_states_t::init:
+			currentState = tacos_states_t::started;
+
+			lockoutCycles = 5000;
+			failsafeCycles = 6000;
+
+		case tacos_states_t::started:
+			currentState = tacos_states_t::flying;
+
+			lockoutCycles = 1000;
+			failsafeCycles = 2000;
+
+		case tacos_states_t::flying:
+			currentState = tacos_states_t::landed;
+
+			lockoutCycles = 5000;
+			failsafeCycles = 6000;
+
+			break;
+		default:
+			break;
+	}
+
+	// Restart the timers.
+	osTimerStart(lockout, lockoutCycles);
+	osTimerStart(failsafe, failsafeCycles);
+}
+
+
+extern "C" void startStateMachine(void*)
+{
+	// Initialize the stateChange event.
+	stateChangeEvent_id = osEventFlagsNew(NULL);
+	STA_ASSERT_MSG(stateChangeEvent_id != NULL, "Failed to initialize state change event!");
+
+	// The timers for catching errors.
+	lockout = osTimerNew(lockoutCallback, osTimerOnce, NULL, NULL);
+	failsafe = osTimerNew(failsafeCallback, osTimerOnce, NULL, NULL);
+
+	// Check if the initialization of the timers was successful.
+	STA_ASSERT_MSG(lockout != 0, "Failed to initialize lockout timer");
+	STA_ASSERT_MSG(failsafe != 0, "Failed to initialize failsafe timer");
+
+	while (true)
+	{
+		if (checkStateChangeConditions())
+		{
+			changeState(TACOS_STATE_CHG_NATURAL);
+		}
+	}
+
+	osThreadExit();
+}
+
+