diff --git a/App/Src/startup.cpp b/App/Src/startup.cpp index 1cd7e5b..a2e0875 100644 --- a/App/Src/startup.cpp +++ b/App/Src/startup.cpp @@ -26,9 +26,9 @@ namespace sta { STA_DEBUG_PRINTLN("Starting manager task!"); - Manager::instance()->registerThread(demo::DummyTask("A"), {0, 1}); - Manager::instance()->registerThread(demo::DummyTask("B"), {1, 2}); - Manager::instance()->registerThread(demo::DummyTask("C"), {2, 3}); + // Manager::instance()->registerThread(demo::DummyTask("A"), {0, 1}); + // Manager::instance()->registerThread(demo::DummyTask("B"), {1, 2}); + // Manager::instance()->registerThread(demo::DummyTask("C"), {2, 3}); } } // namespace tacos } // namespace sta diff --git a/Libs/rtos2-utils b/Libs/rtos2-utils index 5b8c97c..2948c92 160000 --- a/Libs/rtos2-utils +++ b/Libs/rtos2-utils @@ -1 +1 @@ -Subproject commit 5b8c97cf5c4b57702c25c0f7e7bcaea4575a6f7d +Subproject commit 2948c9203e44a5126e6a0067522698964f0ee7c4 diff --git a/Libs/sta-core b/Libs/sta-core index 8e93db1..4dc625f 160000 --- a/Libs/sta-core +++ b/Libs/sta-core @@ -1 +1 @@ -Subproject commit 8e93db18174937562f246af4b1e8d99bd6bbf632 +Subproject commit 4dc625f6e9ebe8db0fc795719a26ee900425b49a diff --git a/Tacos/include/sta/tacos/manager.hpp b/Tacos/include/sta/tacos/manager.hpp index e59ffba..07120d8 100644 --- a/Tacos/include/sta/tacos/manager.hpp +++ b/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 threads_[STA_TACOS_NUM_STATES]; - - class CGuard - { - public: - ~CGuard() - { - if( NULL != Manager::_instance ) - { - delete Manager::_instance; - Manager::_instance = NULL; - } - } - }; }; } // namespace tacos } // namespace sta diff --git a/Tacos/include/sta/tacos/statemachine.hpp b/Tacos/include/sta/tacos/statemachine.hpp index 0a90231..749ab8b 100644 --- a/Tacos/include/sta/tacos/statemachine.hpp +++ b/Tacos/include/sta/tacos/statemachine.hpp @@ -11,6 +11,20 @@ #include #include +#include + +#include + + +#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_ */ diff --git a/Tacos/include/sta/tacos/thread.hpp b/Tacos/include/sta/tacos/thread.hpp index 0b9bf4f..c054f8b 100644 --- a/Tacos/include/sta/tacos/thread.hpp +++ b/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(); /** diff --git a/Tacos/src/manager.cpp b/Tacos/src/manager.cpp index e44ae79..eebdc1f 100644 --- a/Tacos/src/manager.cpp +++ b/Tacos/src/manager.cpp @@ -7,6 +7,7 @@ #include #include +#include namespace sta @@ -69,11 +70,20 @@ namespace sta void Manager::init() { + STA_DEBUG_PRINTLN("INITIALIZING MANAGER!"); + startThreads(Statemachine::instance()->getCurrentState()); } void Manager::func() { + if (true) + { + // STA_DEBUG_PRINTLN("LOOPY LOOP IN MANAGER TASK"); + osDelay(1000); + } + + // Wait for either the termination request or the state change flag. uint32_t flags = osEventFlagsWait(getInstance(), STA_RTOS_THREAD_FLAG_TERMINATE, osFlagsWaitAny, osWaitForever); diff --git a/Tacos/src/startup.cpp b/Tacos/src/startup.cpp index 51e7670..1a9e4fb 100644 --- a/Tacos/src/startup.cpp +++ b/Tacos/src/startup.cpp @@ -69,7 +69,7 @@ namespace sta { onManagerInit(); - Statemachine::instance()->start(); + Manager::instance()->start(); } } // namespace tacos diff --git a/Tacos/src/statemachine.cpp b/Tacos/src/statemachine.cpp index 3cc017e..a4a5114 100644 --- a/Tacos/src/statemachine.cpp +++ b/Tacos/src/statemachine.cpp @@ -7,23 +7,43 @@ #include +#include + + namespace sta { namespace tacos { - Statemachine::Statemachine(){} + Statemachine::Statemachine() + : currentState_{STA_TACOS_INITIAL_STATE}, lockoutTimer_{[](void *){}, NULL}, failsafeTimer_{[](void *){}, NULL} + { + STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES); + } void Statemachine::init() { + STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE"); + /* + lockoutTimer_.setCallback([](void *) { STA_DEBUG_PRINTLN("Lockout timer triggered!"); }, NULL); + lockoutTimer_.start(1000); + + failsafeTimer_.setCallback([](void *) { STA_DEBUG_PRINTLN("Failsafe timer triggered!"); }, NULL); + failsafeTimer_.start(2000); + */ } void Statemachine::func() { + // STA_DEBUG_PRINTLN("LOOPY LOOP IN STATEMACHINE"); + STA_DEBUG_PRINT("Failsafe timer is running: "); + STA_DEBUG_PRINTLN(failsafeTimer_.isRunning()); + + osDelay(1000); } - uint16_t Statemachine::getCurrentState() + uint16_t Statemachine::getCurrentState() const { return currentState_; } diff --git a/Tacos/src/thread.cpp b/Tacos/src/thread.cpp index 09ab927..f821973 100644 --- a/Tacos/src/thread.cpp +++ b/Tacos/src/thread.cpp @@ -19,11 +19,14 @@ namespace sta { TacosThread::TacosThread(const char* name, osPriority_t prio) : RtosThread(RtosHandle(Handle::Deferred(&instance_))), + instance_{ NULL }, attribs_{ .name = name, .priority = prio } {} TacosThread::TacosThread() - : RtosThread(RtosHandle(Handle::Deferred(&instance_))) + : RtosThread(RtosHandle(Handle::Deferred(&instance_))), + instance_{ NULL }, + attribs_{ } {} void TacosThread::entry_point(void* arg) @@ -85,6 +88,7 @@ namespace sta } void TacosThread::init(){} + void TacosThread::func(){} TacosThread::~TacosThread(){}