diff --git a/src/boards/Groundstation/Automated/Leds/Leds.cpp b/src/boards/Groundstation/Automated/Leds/Leds.cpp index aa4ba87a588af45d7e204621f9b2b3cce0532db1..e01d49c24981b62eaab31bd78d97a8a5871b5099 100644 --- a/src/boards/Groundstation/Automated/Leds/Leds.cpp +++ b/src/boards/Groundstation/Automated/Leds/Leds.cpp @@ -22,66 +22,163 @@ #include "Leds.h" -#include <miosix.h> +#include <mutex> +using namespace std; using namespace miosix; using namespace Boardcore; namespace Antennas { +Leds::Leds(TaskScheduler* scheduler) : scheduler(scheduler) +{ + *led_ref(LedColor::RED) = LedState::OFF; + *led_ref(LedColor::YELLOW) = LedState::OFF; + *led_ref(LedColor::ORANGE) = LedState::OFF; + *led_ref(LedColor::GREEN) = LedState::OFF; +} + bool Leds::start() { + size_t result; + bool ok = true; + // turn off all leds ledOff(); - if (!ActiveObject::start()) - { - return false; - } + // start the blinking thread for red led + result = scheduler->addTask( + std::bind(&Leds::led_thread, this, LedColor::RED, 100), 0, + TaskScheduler::Policy::ONE_SHOT); + ok &= result != 0; + + // start the blinking thread for yellow led + result = scheduler->addTask( + std::bind(&Leds::led_thread, this, LedColor::YELLOW, 50), 0, + TaskScheduler::Policy::ONE_SHOT); + ok &= result != 0; + + // start the blinking thread for orange led + result = scheduler->addTask( + std::bind(&Leds::led_thread, this, LedColor::ORANGE, 50), 0, + TaskScheduler::Policy::ONE_SHOT); + ok &= result != 0; - return true; + // start the blinking thread for green led + result = scheduler->addTask( + std::bind(&Leds::led_thread, this, LedColor::GREEN, 50), 0, + TaskScheduler::Policy::ONE_SHOT); + ok &= result != 0; + + return ok; } -void Leds::run() +void Leds::led_on(LedColor color) { - while (!shouldStop()) +#ifdef _BOARD_STM32F767ZI_AUTOMATED_ANTENNAS + switch (color) { - // we avoid using mutex to avoid waiting for the release of it - // data races here are not a problem - if (blinking_red) - { + case LedColor::RED: userLed4::high(); - Thread::sleep(100); - userLed4::low(); - Thread::sleep(100); - } - else if (yellow_blink) - { + break; + case LedColor::YELLOW: led2On(); - Thread::sleep(50); - led2Off(); - Thread::sleep(50); - } - else if (orange_blinking) - { + break; + case LedColor::ORANGE: led3On(); - Thread::sleep(50); + break; + case LedColor::GREEN: + led1On(); + break; + } +#endif +} + +void Leds::led_off(LedColor color) +{ +#ifdef _BOARD_STM32F767ZI_AUTOMATED_ANTENNAS + switch (color) + { + case LedColor::RED: + userLed4::low(); + break; + case LedColor::YELLOW: + led2Off(); + break; + case LedColor::ORANGE: led3Off(); - Thread::sleep(50); - } + break; + case LedColor::GREEN: + led1Off(); + break; } +#endif } -void Leds::errorLoop() +/** + * @brief Thread function that actuate leds based on state + * @param color led color to actuate + * @param blinking_interval blinking time interval [ms] + */ +void Leds::led_thread(LedColor color, uint32_t blinking_interval) { while (true) { - userLed4::high(); - Thread::sleep(100); - userLed4::low(); - Thread::sleep(100); + unique_lock<mutex> lock(*mutex_ref(color)); + cv_ref(color)->wait(lock); + switch (*led_ref(color)) + { + case LedState::BLINKING: + { + do + { + led_on(color); + Thread::sleep(blinking_interval); + led_off(color); + Thread::sleep(blinking_interval); + } while (*led_ref(color) == LedState::BLINKING); + break; + } + case LedState::ON: + { + led_on(color); + } + case LedState::OFF: + { + led_off(color); + } + } } } +void Leds::set_blinking(LedColor color) +{ + lock_guard<mutex> lock(*mutex_ref(color)); + *led_ref(color) = LedState::BLINKING; + cv_ref(color)->notify_one(); +} + +void Leds::set_on(LedColor color) +{ + lock_guard<mutex> lock(*mutex_ref(color)); + *led_ref(color) = LedState::ON; + cv_ref(color)->notify_one(); +} + +void Leds::set_off(LedColor color) +{ + lock_guard<mutex> lock(*mutex_ref(color)); + *led_ref(color) = LedState::OFF; + cv_ref(color)->notify_one(); +} + +void Leds::endless_blink(LedColor color) +{ + lock_guard<mutex> lock(*mutex_ref(color)); + *led_ref(color) = LedState::BLINKING; + cv_ref(color)->notify_one(); + Thread::wait(); // wait forever +} + } // namespace Antennas diff --git a/src/boards/Groundstation/Automated/Leds/Leds.h b/src/boards/Groundstation/Automated/Leds/Leds.h index 605569e8ef34822e8bc4e1b388f76520d43d8b34..f1f2d358dc01115a7ebfd60e94978dbf97aa6b33 100644 --- a/src/boards/Groundstation/Automated/Leds/Leds.h +++ b/src/boards/Groundstation/Automated/Leds/Leds.h @@ -22,24 +22,32 @@ #pragma once -#include <ActiveObject.h> -#include <miosix.h> +#include <scheduler/TaskScheduler.h> +#include <condition_variable> +#include <map> +#include <mutex> #include <utils/ModuleManager/ModuleManager.hpp> namespace Antennas { +enum class LedColor : uint8_t +{ + RED = 0, + YELLOW, + ORANGE, + GREEN +}; + /** * @brief Utility to handle blinking leds with non-blocking sleep * (useful for state machines states that need to blink leds without blocking) */ -class Leds : public Boardcore::Module, protected Boardcore::ActiveObject +class Leds : public Boardcore::Module { public: - Leds() {} - - ~Leds() { ActiveObject::stop(); } + Leds(Boardcore::TaskScheduler* scheduler); /** * @brief Start the blinking LED thread @@ -47,80 +55,56 @@ public: bool start(); /** - * @brief start blinking red led + * @brief non-blocking action to set a led to blink in a loop */ - void start_blinking_red() { blinking_red = true; } + void set_blinking(LedColor color); /** - * @brief stop blinking red led + * @brief blocking action to set on a led */ - void stop_blinking_red() { blinking_red = false; } + void set_on(LedColor color); /** - * @brief turn on green led + * @brief blocking action to set off a led */ - void turn_on_green() { miosix::led1On(); } + void set_off(LedColor color); /** - * @brief start blinking yellow led + * @brief blocking action to blink endlessly a led */ - void start_blinking_yellow() { yellow_blink = true; } + void endless_blink(LedColor color); - /** - * @brief stop blinking yellow led - */ - void stop_blinking_yellow() +protected: + enum class LedState { - yellow_blink = false; - miosix::Thread::sleep(101); // to avoid data races with led2On/Off - } + OFF, + ON, + BLINKING + }; - /** - * @brief turn on yellow led - */ - void turn_on_yellow() { miosix::led2On(); } - - /** - * @brief turn off yellow led - */ - void turn_off_yellow() { miosix::led2Off(); } - - /** - * @brief start blinking orange led - */ - void start_blinking_orange() { orange_blinking = true; } + void led_on(LedColor color); + void led_off(LedColor color); + void led_thread(LedColor color, uint32_t ms_interval); - /** - * @brief stop blinking orange led - */ - void stop_blinking_orange() + LedState* led_ref(LedColor color) { - orange_blinking = false; - miosix::Thread::sleep(101); // to avoid data races with led3On/Off + return &leds[static_cast<uint8_t>(color)]; } - /** - * @brief turn on orange led - */ - void turn_on_orange() { miosix::led3On(); } - - /** - * @brief turn off orange led - */ - void turn_off_orange() { miosix::led3Off(); } - - /** - * @brief Blink the red led in a loop, blocking the thread. - */ - static void errorLoop(); + std::mutex* mutex_ref(LedColor color) + { + return &led_mutex[static_cast<uint8_t>(color)]; + } -protected: - void run() override; + std::condition_variable* cv_ref(LedColor color) + { + return &cv[static_cast<uint8_t>(color)]; + } -private: - std::atomic_bool blinking_red{false}; - std::atomic_bool yellow_blink{false}; - std::atomic_bool orange_blinking{false}; + Boardcore::TaskScheduler* scheduler; + LedState leds[4]; + std::mutex led_mutex[4]; + std::condition_variable cv[4]; }; } // namespace Antennas diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp index d98b1e1f67d476bffa7923592a4851ba3ff5d935..e4f09483b293ee9ec60c1f8eafae90a6c1e9cae7 100644 --- a/src/boards/Groundstation/Automated/SMController/SMController.cpp +++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp @@ -370,12 +370,13 @@ State SMController::state_init_error(const Event& event) case EV_ENTRY: { logStatus(SMControllerState::INIT_ERROR); - ModuleManager::getInstance().get<Leds>()->start_blinking_red(); + ModuleManager::getInstance().get<Leds>()->set_blinking( + LedColor::RED); return HANDLED; } case EV_EXIT: { - ModuleManager::getInstance().get<Leds>()->stop_blinking_red(); + ModuleManager::getInstance().get<Leds>()->set_off(LedColor::RED); return HANDLED; } case EV_EMPTY: @@ -408,7 +409,7 @@ State SMController::state_init_done(const Event& event) case EV_ENTRY: { logStatus(SMControllerState::INIT_DONE); - ModuleManager::getInstance().get<Leds>()->turn_on_green(); + ModuleManager::getInstance().get<Leds>()->set_on(LedColor::GREEN); return HANDLED; } case EV_EXIT: @@ -581,14 +582,14 @@ State SMController::state_fix_antennas(const Event& event) case EV_ENTRY: { logStatus(SMControllerState::FIX_ANTENNAS); - ModuleManager::getInstance().get<Leds>()->start_blinking_orange(); + ModuleManager::getInstance().get<Leds>()->set_blinking( + LedColor::ORANGE); return HANDLED; } case EV_EXIT: { auto* leds = ModuleManager::getInstance().get<Leds>(); - leds->stop_blinking_orange(); - leds->turn_on_orange(); + leds->set_on(LedColor::ORANGE); return HANDLED; } case EV_EMPTY: @@ -621,13 +622,14 @@ State SMController::state_fix_rocket(const Event& event) case EV_ENTRY: { logStatus(SMControllerState::FIX_ROCKET); - ModuleManager::getInstance().get<Leds>()->start_blinking_yellow(); + ModuleManager::getInstance().get<Leds>()->set_blinking( + LedColor::YELLOW); return HANDLED; } case EV_EXIT: { auto* leds = ModuleManager::getInstance().get<Leds>(); - leds->stop_blinking_yellow(); + leds->set_off(LedColor::YELLOW); // init the follower before leaving the state // (compute initial arp-rocket distance and bearing) @@ -636,7 +638,7 @@ State SMController::state_fix_rocket(const Event& event) LOG_ERR(logger, "Follower initialization failed"); } - leds->turn_on_yellow(); + leds->set_on(LedColor::YELLOW); return HANDLED; } case EV_EMPTY: @@ -773,13 +775,14 @@ State SMController::state_fix_rocket_nf(const Event& event) case EV_ENTRY: { logStatus(SMControllerState::FIX_ROCKET_NF); - ModuleManager::getInstance().get<Leds>()->start_blinking_yellow(); + ModuleManager::getInstance().get<Leds>()->set_blinking( + LedColor::YELLOW); return HANDLED; } case EV_EXIT: { auto* leds = ModuleManager::getInstance().get<Leds>(); - leds->stop_blinking_yellow(); + leds->set_off(LedColor::YELLOW); // init the follower before leaving the state // (compute initial arp-rocket distance and bearing) @@ -788,7 +791,7 @@ State SMController::state_fix_rocket_nf(const Event& event) LOG_ERR(logger, "Follower initialization failed"); } - leds->turn_on_yellow(); + leds->set_on(LedColor::YELLOW); return HANDLED; } case EV_EMPTY: diff --git a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp index 3a562147b2321214b6fca1db1924833291e27bc6..42f3295020ca4d1e09c7f0cb734be903698e6c7a 100644 --- a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp +++ b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp @@ -47,7 +47,7 @@ if (!_fun()) \ { \ LOG_ERR(logger, "Failed to start module " name); \ - Leds::errorLoop(); \ + leds->endless_blink(LedColor::RED); \ } \ else \ { \ @@ -97,17 +97,18 @@ int main() }); ButtonHandler::getInstance().start(); - TaskScheduler *scheduler = new TaskScheduler(); - Hub *hub = new Hub(); - Buses *buses = new Buses(); - Serial *serial = new Serial(); - RadioMain *radio_main = new RadioMain(); - BoardStatus *board_status = new BoardStatus(); - Actuators *actuators = new Actuators(); - Sensors *sensors = new Sensors(); - SMController *sm = new SMController(scheduler); - Ethernet *ethernet = new Ethernet(); - Leds *leds = new Leds(); + TaskScheduler *scheduler_low = new TaskScheduler(0); + TaskScheduler *scheduler_high = new TaskScheduler(); + Leds *leds = new Leds(scheduler_low); + Hub *hub = new Hub(); + Buses *buses = new Buses(); + Serial *serial = new Serial(); + RadioMain *radio_main = new RadioMain(); + BoardStatus *board_status = new BoardStatus(); + Actuators *actuators = new Actuators(); + Sensors *sensors = new Sensors(); + SMController *sm = new SMController(scheduler_high); + Ethernet *ethernet = new Ethernet(); // Inserting Modules { // TODO remove this scope (improve readability) @@ -126,7 +127,7 @@ int main() if (!ok) { LOG_ERR(logger, "Failed to insert all modules!\n"); - Leds::errorLoop(); + leds->endless_blink(LedColor::RED); } else { @@ -139,7 +140,8 @@ int main() #ifndef NO_SD_LOGGING START_MODULE("Logger", [&] { return Logger::getInstance().start(); }); #endif - START_MODULE("Scheduler", [&] { return scheduler->start(); }); + START_MODULE("Scheduler", [&] { return scheduler_low->start(); }); + START_MODULE("Scheduler", [&] { return scheduler_high->start(); }); START_MODULE("Serial", [&] { return serial->start(); }); START_MODULE("Main Radio", [&] { return radio_main->start(); }); START_MODULE("Ethernet", [&] { return ethernet->start(); });