From c02bfd24a87abddbdec281bfe697efab6e50c3fe Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Sun, 12 May 2024 13:47:49 +0200
Subject: [PATCH] [ARP] Refactor of Leds class
- This introduce compiler conditional compilation (based on
architecture), this fix previous issue with building
- Leds now is thread safe, uses a low priority scheduler and keep tasks
alive if leds needs non-blocking blinking
---
.../Groundstation/Automated/Leds/Leds.cpp | 161 ++++++++++++++----
.../Groundstation/Automated/Leds/Leds.h | 106 +++++-------
.../Automated/SMController/SMController.cpp | 27 +--
.../Automated/automated-antennas-entry.cpp | 30 ++--
4 files changed, 205 insertions(+), 119 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Leds/Leds.cpp b/src/boards/Groundstation/Automated/Leds/Leds.cpp
index aa4ba87a5..e01d49c24 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 605569e8e..f1f2d358d 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 d98b1e1f6..e4f09483b 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 3a562147b..42f329502 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(); });
--
GitLab