diff --git a/App/Src/startup.cpp b/App/Src/startup.cpp index 3467975..f4e3274 100644 --- a/App/Src/startup.cpp +++ b/App/Src/startup.cpp @@ -28,10 +28,10 @@ namespace sta { STA_DEBUG_PRINTLN("Starting manager task!"); - Manager::instance()->registerThread(std::make_shared("A"), {0, 1}); - Manager::instance()->registerThread(std::make_shared("B"), {0, 2}); - Manager::instance()->registerThread(std::make_shared("C"), {0, 1}); - Manager::instance()->registerThread(std::make_shared("D"), {0, 2}); + Manager::instance()->registerThread(std::make_shared("0"), {0}); + Manager::instance()->registerThread(std::make_shared("1"), {1}); + Manager::instance()->registerThread(std::make_shared("2"), {2}); + Manager::instance()->registerThread(std::make_shared("3"), {3}); } } // namespace tacos } // namespace sta diff --git a/Libs/rtos2-utils b/Libs/rtos2-utils index 1028264..d591560 160000 --- a/Libs/rtos2-utils +++ b/Libs/rtos2-utils @@ -1 +1 @@ -Subproject commit 1028264eefaf0c384bab4391c6151951bd76842e +Subproject commit d591560c95a4eae525cbbbdb4e4bc15642852473 diff --git a/Tacos/include/sta/tacos/statemachine.hpp b/Tacos/include/sta/tacos/statemachine.hpp index 443d4b4..8a3ab1f 100644 --- a/Tacos/include/sta/tacos/statemachine.hpp +++ b/Tacos/include/sta/tacos/statemachine.hpp @@ -20,9 +20,14 @@ # define STA_TACOS_INITIAL_STATE 0 #endif +#define TACOS_STATE_TRANSITION_FLAG 0x00000001U +#define TACOS_STATE_CHANGE_FORCED_FLAG 0x00000002U +#define TACOS_STATE_CHANGE_ALL_FLAG 0x00000003U + #include #include +#include #include #include @@ -35,6 +40,8 @@ namespace sta class Statemachine : public TacosThread { public: + static RtosEvent* stateChangeEvent; + static Statemachine* instance() { static CGuard g; @@ -43,6 +50,7 @@ namespace sta { // Create a the manager singleton instance. Statemachine::_instance = new Statemachine(); + Statemachine::stateChangeEvent = new RtosEvent(); } return _instance; @@ -52,10 +60,17 @@ namespace sta void func() override; + /** + * @brief Returns the statemachine's current state. + */ uint16_t getCurrentState() const; - void setStateTransitionFunction(uint16_t (*function)(uint16_t)); + /** + * @brief Registers a new state transition function. + */ + void setStateTransitionFunction(std::function function); + void forceStateTransition(uint16_t state); private: static Statemachine * _instance; @@ -67,7 +82,9 @@ namespace sta if( NULL != Statemachine::_instance ) { delete Statemachine::_instance; + delete Statemachine::stateChangeEvent; Statemachine::_instance = NULL; + Statemachine::stateChangeEvent = NULL; } } }; @@ -77,10 +94,6 @@ namespace sta Statemachine(const Statemachine&); ~Statemachine() {} - - private: - static void forceStateChange(void * arg); - private: uint16_t currentState_; RtosTimer lockoutTimer_; diff --git a/Tacos/src/manager.cpp b/Tacos/src/manager.cpp index cf52ed2..32caca7 100644 --- a/Tacos/src/manager.cpp +++ b/Tacos/src/manager.cpp @@ -9,6 +9,8 @@ #include #include +#include + namespace sta { @@ -39,22 +41,23 @@ namespace sta void Manager::stopThreads(uint16_t state) { - uint16_t currentState = Statemachine::instance()->getCurrentState(); - - for (uint16_t state = 0; state < STA_TACOS_NUM_STATES; ++state) + for (uint16_t other = 0; other < STA_TACOS_NUM_STATES; ++other) { - if (state == currentState) + if (other == state) { continue; } - for (std::shared_ptr thread : threads_[state]) + for (std::shared_ptr thread : threads_[other]) { // If the thread is currently running but not part of the set of threads that should be running... - if (thread->isRunning() && threads_[currentState].count(thread) == 0) + if (thread->isRunning() && threads_[state].count(thread) == 0) { // ...politely request termination. thread->requestTermination(); + + STA_DEBUG_PRINT("KILLING "); + STA_DEBUG_PRINTLN(thread->getName()); } } } @@ -75,14 +78,9 @@ namespace sta void Manager::func() { - // Wait for either the termination request or the state change flag. - uint32_t flags = osEventFlagsWait(getInstance(), STA_RTOS_THREAD_FLAG_TERMINATE, osFlagsWaitAny, osWaitForever); + Statemachine::stateChangeEvent->wait(TACOS_STATE_CHANGE_ALL_FLAG, osWaitForever); - if ((flags & STA_RTOS_THREAD_FLAG_TERMINATE) != 0) - { - // The loop implemented by the TacosThread class should handle termination. - return; - } + STA_DEBUG_PRINTLN("UPDATING THREADS"); // Start all new tasks and stop all the tasks that aren't supposed to be running. updateThreads(); diff --git a/Tacos/src/statemachine.cpp b/Tacos/src/statemachine.cpp index 9d607a4..8390f9c 100644 --- a/Tacos/src/statemachine.cpp +++ b/Tacos/src/statemachine.cpp @@ -18,7 +18,8 @@ namespace sta : TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY}, currentState_{STA_TACOS_INITIAL_STATE}, lockoutTimer_{[](void *){}, nullptr}, - failsafeTimer_{[](void *){}, nullptr} + failsafeTimer_{[](void *){}, nullptr}, + transitionFunc_{[](uint16_t) -> uint16_t { return Statemachine::instance()->getCurrentState(); }} { STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES); } @@ -28,15 +29,14 @@ namespace sta lockoutTimer_.setCallback([](void *) { }, nullptr); lockoutTimer_.start(5000); - STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE"); - - failsafeTimer_.setCallback([](void * arg) { }, nullptr); + failsafeTimer_.setCallback([](void *) { + Statemachine::instance()->forceStateTransition(1); + }, nullptr); failsafeTimer_.start(10000); } void Statemachine::func() { - /* uint16_t next = transitionFunc_(currentState_); STA_ASSERT(next < STA_TACOS_NUM_STATES); @@ -45,10 +45,10 @@ namespace sta { currentState_ = next; - // TODO: Emit state transition event. - }*/ - STA_DEBUG_PRINTLN(lockoutTimer_.isRunning() ? "[RUNNING]" : "[STOPPED]"); - osDelay(1000); + Statemachine::stateChangeEvent->set(TACOS_STATE_TRANSITION_FLAG); + } + + Statemachine::stateChangeEvent->clear(TACOS_STATE_CHANGE_ALL_FLAG); } uint16_t Statemachine::getCurrentState() const @@ -56,14 +56,23 @@ namespace sta return currentState_; } - void Statemachine::setStateTransitionFunction(uint16_t (*function)(uint16_t)) + void Statemachine::setStateTransitionFunction(std::function function) { STA_ASSERT(function != nullptr); transitionFunc_ = function; } + void Statemachine::forceStateTransition(uint16_t state) + { + currentState_ = state; + + Statemachine::stateChangeEvent->set(TACOS_STATE_CHANGE_FORCED_FLAG); + Statemachine::stateChangeEvent->clear(TACOS_STATE_CHANGE_ALL_FLAG); + } + Statemachine* Statemachine::_instance = nullptr; + RtosEvent* Statemachine::stateChangeEvent = nullptr; } // namespace tacos } // namespace sta diff --git a/Tacos/src/thread.cpp b/Tacos/src/thread.cpp index e0e3d5f..96bcf7d 100644 --- a/Tacos/src/thread.cpp +++ b/Tacos/src/thread.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -66,9 +67,13 @@ namespace sta void TacosThread::loop() { + // The thread has to wait until the startup has been completed. + rtos::waitForStartupEvent(); + init(); - while ((osEventFlagsGet(instance_) & STA_RTOS_THREAD_FLAG_TERMINATE) == 0) + // Run the thread until the termination flag is set. + while ((getFlags() & STA_RTOS_THREAD_FLAG_TERMINATE) == 0) { func(); }