From 4bd14b53b27d09f9325e16c3875559dd967b2169 Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Thu, 23 May 2024 22:14:13 +0200
Subject: [PATCH] [ARP] Refactor of Leds as single thread
needs testing
---
.../Groundstation/Automated/Leds/Leds.cpp | 193 ++++++++----------
.../Groundstation/Automated/Leds/Leds.h | 85 ++++----
.../Automated/SMController/SMController.cpp | 16 +-
.../Automated/automated-antennas-entry.cpp | 6 +-
4 files changed, 138 insertions(+), 162 deletions(-)
diff --git a/src/boards/Groundstation/Automated/Leds/Leds.cpp b/src/boards/Groundstation/Automated/Leds/Leds.cpp
index ff5069823..ca775eb36 100644
--- a/src/boards/Groundstation/Automated/Leds/Leds.cpp
+++ b/src/boards/Groundstation/Automated/Leds/Leds.cpp
@@ -22,20 +22,99 @@
#include "Leds.h"
-#include <mutex>
-
using namespace Boardcore;
namespace Antennas
{
-LedThread::LedThread(LedColor color)
- : ActiveObject(miosix::STACK_DEFAULT_FOR_PTHREAD, 0), color(color),
- state(LedState::OFF)
+Leds::Leds(TaskScheduler* scheduler) : scheduler(scheduler)
{
+ leds_state.fill(LedState::OFF);
+ led_steps.fill(0);
}
-void LedThread::ledOn(LedColor color)
+bool Leds::start()
+{
+ size_t result;
+ bool ok = true;
+
+ // turn off all leds
+ miosix::ledOff();
+
+ result = scheduler->addTask(std::bind(&Leds::update, this),
+ LED_BLINK_FAST_PERIOD_MS,
+ TaskScheduler::Policy::RECOVER);
+ ok &= result;
+
+ return ok;
+}
+
+void Leds::update()
+{
+ LedState state;
+ LedColor color;
+
+ for (size_t i = 0; i < leds_state.size(); i++)
+ {
+ state = leds_state[i];
+ color = static_cast<LedColor>(i);
+
+ switch (state)
+ {
+ case LedState::BLINK_SLOW:
+ if (led_steps[i]++ == 1)
+ {
+ ledToggle(color);
+ led_steps[i] = 0;
+ }
+ break;
+ case LedState::BLINK_FAST:
+ {
+ ledToggle(color);
+ break;
+ }
+ case LedState::ON:
+ {
+ ledOn(color);
+ break;
+ }
+ case LedState::OFF:
+ {
+ ledOff(color);
+ break;
+ }
+ }
+ }
+}
+
+void Leds::setFastBlink(LedColor color)
+{
+ *ledRef(color) = LedState::BLINK_FAST;
+}
+
+void Leds::setSlowBlink(LedColor color)
+{
+ *ledRef(color) = LedState::BLINK_SLOW;
+}
+
+void Leds::setOn(LedColor color) { *ledRef(color) = LedState::ON; }
+
+void Leds::setOff(LedColor color) { *ledRef(color) = LedState::OFF; }
+
+void Leds::endlessBlink(LedColor color)
+{
+ setSlowBlink(color);
+ miosix::Thread::wait(); // wait forever
+}
+
+void Leds::ledToggle(LedColor color)
+{
+ size_t i = static_cast<size_t>(color);
+ led_toggles[i] ? ledOn(color) : ledOff(color);
+ led_toggles[i] = !led_toggles[i];
+}
+
+void Leds::ledOn(LedColor color)
{
#ifdef _BOARD_STM32F767ZI_AUTOMATED_ANTENNAS
switch (color)
@@ -72,7 +151,7 @@ void LedThread::ledOn(LedColor color)
#endif
}
-void LedThread::ledOff(LedColor color)
+void Leds::ledOff(LedColor color)
{
#ifdef _BOARD_STM32F767ZI_AUTOMATED_ANTENNAS
switch (color)
@@ -109,104 +188,4 @@ void LedThread::ledOff(LedColor color)
#endif
}
-void LedThread::run()
-{
- LedState old = state;
- while (true)
- {
- std::unique_lock<std::mutex> lock(mutex);
- cv.wait(lock, [&] { return state != old; });
- old = state;
- switch (state)
- {
- case LedState::BLINKING:
- {
- do
- {
- ledOn(color);
- miosix::Thread::sleep(blinking_interval);
- ledOff(color);
- miosix::Thread::sleep(blinking_interval);
- } while (state == LedState::BLINKING);
- break;
- }
- case LedState::ON:
- {
- ledOn(color);
- break;
- }
- case LedState::OFF:
- {
- ledOff(color);
- break;
- }
- }
- }
-}
-
-void LedThread::setBlinking(uint32_t ms_interval)
-{
- {
- std::lock_guard<std::mutex> lock(mutex);
- state = LedState::BLINKING;
- }
- cv.notify_one();
-}
-
-void LedThread::setOn()
-{
- {
- std::lock_guard<std::mutex> lock(mutex);
- state = LedState::ON;
- }
- cv.notify_one();
-}
-
-void LedThread::setOff()
-{
- {
- std::lock_guard<std::mutex> lock(mutex);
- state = LedState::OFF;
- }
- cv.notify_one();
-}
-
-Leds::Leds()
-{
- for (size_t i = 0; i < leds.size(); i++)
- {
- leds[i] = std::make_unique<LedThread>(static_cast<LedColor>(i));
- }
-}
-
-bool Leds::start()
-{
- bool ok = true;
-
- // turn off all leds
- miosix::ledOff();
-
- for (size_t i = 0; i < leds.size(); i++)
- {
- ok &= leds[i]->start();
- }
-
- return ok;
-}
-
-void Leds::setBlinking(LedColor color, uint32_t ms_interval)
-{
- ledRef(color)->setBlinking(ms_interval);
-}
-
-void Leds::setOn(LedColor color) { ledRef(color)->setOn(); }
-
-void Leds::setOff(LedColor color) { ledRef(color)->setOff(); }
-
-void Leds::endlessBlink(LedColor color)
-{
- ledRef(color)->setBlinking(100);
- miosix::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 27830e4c9..1c81ea9f2 100644
--- a/src/boards/Groundstation/Automated/Leds/Leds.h
+++ b/src/boards/Groundstation/Automated/Leds/Leds.h
@@ -23,12 +23,15 @@
#pragma once
#include <ActiveObject.h>
+#include <scheduler/TaskScheduler.h>
#include <array>
-#include <condition_variable>
#include <mutex>
#include <utils/ModuleManager/ModuleManager.hpp>
+constexpr uint32_t LED_BLINK_FAST_PERIOD_MS = 50;
+constexpr uint32_t LED_BLINK_SLOW_PERIOD_MS = 100;
+
namespace Antennas
{
@@ -40,45 +43,6 @@ enum class LedColor : uint8_t
GREEN
};
-class LedThread : public Boardcore::ActiveObject
-{
-public:
- explicit LedThread(LedColor color);
-
- /**
- * @brief non-blocking action to set a led to blink in a loop
- */
- void setBlinking(uint32_t ms_interval);
-
- /**
- * @brief non-blocking action to set on the led
- */
- void setOn();
-
- /**
- * @brief non-blocking action to set off the led
- */
- void setOff();
-
-private:
- enum class LedState : uint8_t
- {
- OFF = 0,
- ON,
- BLINKING
- };
-
- void ledOn(LedColor color);
- void ledOff(LedColor color);
- void run() override;
-
- LedColor color;
- LedState state;
- std::mutex mutex;
- std::condition_variable cv;
- uint32_t blinking_interval = 100; // [ms]
-};
-
/**
* @brief Utility to handle blinking leds with non-blocking sleep
* (useful for state machines states that need to blink leds without blocking)
@@ -86,7 +50,7 @@ private:
class Leds : public Boardcore::Module
{
public:
- explicit Leds();
+ explicit Leds(Boardcore::TaskScheduler* scheduler);
/**
* @brief Start all the blinking LED thread
@@ -96,7 +60,12 @@ public:
/**
* @brief non-blocking action to set a led to blink in a loop
*/
- void setBlinking(LedColor color, uint32_t ms_interval);
+ void setFastBlink(LedColor color);
+
+ /**
+ * @brief non-blocking action to set a led to blink in a loop
+ */
+ void setSlowBlink(LedColor color);
/**
* @brief non-blocking action to set on a led
@@ -114,12 +83,38 @@ public:
void endlessBlink(LedColor color);
private:
- LedThread* ledRef(LedColor color)
+ enum class LedState : uint8_t
+ {
+ OFF = 0,
+ ON,
+ BLINK_SLOW,
+ BLINK_FAST,
+ };
+
+ void ledOn(LedColor color);
+ void ledOff(LedColor color);
+ void ledToggle(LedColor color);
+
+ /**
+ * @brief Update routine called by the scheduler
+ * @details This function is called by the scheduler to update the leds
+ * state and blink them accordingly
+ */
+ void update();
+
+ LedState* ledRef(LedColor color)
{
- return leds[static_cast<uint8_t>(color)].get();
+ return &leds_state[static_cast<uint8_t>(color)];
}
- std::array<std::unique_ptr<LedThread>, 4> leds;
+ // scheduler to run the update blink function
+ Boardcore::TaskScheduler* scheduler;
+ // toggles to allow led to blink
+ std::array<bool, 4> led_toggles;
+ // counter to keep track of slow blink
+ std::array<uint32_t, 4> led_steps;
+ // state of the leds
+ std::array<LedState, 4> leds_state;
};
} // namespace Antennas
diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp
index 817829659..58676053a 100644
--- a/src/boards/Groundstation/Automated/SMController/SMController.cpp
+++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp
@@ -370,8 +370,8 @@ State SMController::state_init_error(const Event& event)
case EV_ENTRY:
{
logStatus(SMControllerState::INIT_ERROR);
- ModuleManager::getInstance().get<Leds>()->setBlinking(LedColor::RED,
- 100);
+ ModuleManager::getInstance().get<Leds>()->setSlowBlink(
+ LedColor::RED);
return HANDLED;
}
case EV_EXIT:
@@ -582,8 +582,8 @@ State SMController::state_fix_antennas(const Event& event)
case EV_ENTRY:
{
logStatus(SMControllerState::FIX_ANTENNAS);
- ModuleManager::getInstance().get<Leds>()->setBlinking(
- LedColor::ORANGE, 50);
+ ModuleManager::getInstance().get<Leds>()->setFastBlink(
+ LedColor::ORANGE);
return HANDLED;
}
case EV_EXIT:
@@ -622,8 +622,8 @@ State SMController::state_fix_rocket(const Event& event)
case EV_ENTRY:
{
logStatus(SMControllerState::FIX_ROCKET);
- ModuleManager::getInstance().get<Leds>()->setBlinking(
- LedColor::YELLOW, 50);
+ ModuleManager::getInstance().get<Leds>()->setFastBlink(
+ LedColor::YELLOW);
return HANDLED;
}
case EV_EXIT:
@@ -775,8 +775,8 @@ State SMController::state_fix_rocket_nf(const Event& event)
case EV_ENTRY:
{
logStatus(SMControllerState::FIX_ROCKET_NF);
- ModuleManager::getInstance().get<Leds>()->setBlinking(
- LedColor::YELLOW, 50);
+ ModuleManager::getInstance().get<Leds>()->setFastBlink(
+ LedColor::YELLOW);
return HANDLED;
}
case EV_EXIT:
diff --git a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
index 6cb204db0..3cf568bae 100644
--- a/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
+++ b/src/entrypoints/Groundstation/Automated/automated-antennas-entry.cpp
@@ -97,8 +97,9 @@ int main()
});
ButtonHandler::getInstance().start();
+ TaskScheduler *scheduler_low = new TaskScheduler(0);
TaskScheduler *scheduler_high = new TaskScheduler();
- Leds *leds = new Leds();
+ Leds *leds = new Leds(scheduler_low);
Hub *hub = new Hub();
Buses *buses = new Buses();
Serial *serial = new Serial();
@@ -139,7 +140,8 @@ int main()
#ifndef NO_SD_LOGGING
START_MODULE("Logger", [&] { return Logger::getInstance().start(); });
#endif
- START_MODULE("Scheduler", [&] { return scheduler_high->start(); });
+ START_MODULE("Scheduler Low", [&] { return scheduler_low->start(); });
+ START_MODULE("Scheduler High", [&] { 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