diff --git a/App/Inc/tasks/statemachine.hpp b/App/Inc/tasks/statemachine.hpp new file mode 100644 index 0000000..e045353 --- /dev/null +++ b/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_ */ diff --git a/App/Src/tasks/manager.cpp b/App/Src/tasks/manager.cpp index 6a76842..ca2f36a 100644 --- a/App/Src/tasks/manager.cpp +++ b/App/Src/tasks/manager.cpp @@ -6,60 +6,117 @@ */ +#include +#include + #include #include #include #include - #include -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 running; + + // A list of states for which this task should be running. + std::list states; +}; + +std::list 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(); diff --git a/App/Src/tasks/statemachine.cpp b/App/Src/tasks/statemachine.cpp new file mode 100644 index 0000000..e3326bb --- /dev/null +++ b/App/Src/tasks/statemachine.cpp @@ -0,0 +1,107 @@ +/* + * state_machine.cpp + * + * Created on: Sep 4, 2023 + * Author: Dario + */ + +#include + +#include +#include + + +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(); +} + +