From 2c585d929f8d5c34cdb56f418bab44ded0811a15 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 18 Jun 2024 19:00:07 +0200
Subject: [PATCH] [Payload] Use references and getter functions in
 BoardScheduler

---
 cmake/dependencies.cmake                      |  1 -
 src/boards/Payload/Actuators/Actuators.cpp    |  6 +-
 src/boards/Payload/Actuators/Actuators.h      | 10 +--
 .../AltitudeTrigger/AltitudeTrigger.cpp       |  6 +-
 .../Payload/AltitudeTrigger/AltitudeTrigger.h | 18 +++--
 src/boards/Payload/BoardScheduler.cpp         | 66 -------------------
 src/boards/Payload/BoardScheduler.h           | 51 +++++++++-----
 src/boards/Payload/CanHandler/CanHandler.cpp  | 19 +++---
 src/boards/Payload/CanHandler/CanHandler.h    | 12 ++--
 .../FlightStatsRecorder.cpp                   |  6 +-
 .../FlightStatsRecorder/FlightStatsRecorder.h |  4 +-
 src/boards/Payload/Radio/Radio.cpp            | 15 ++---
 src/boards/Payload/Radio/Radio.h              |  4 +-
 src/boards/Payload/Sensors/Sensors.cpp        |  8 +--
 src/boards/Payload/Sensors/Sensors.h          |  4 +-
 .../NASController/NASController.cpp           | 10 +--
 .../NASController/NASController.h             |  4 +-
 .../WingController/WingController.cpp         |  5 +-
 .../WingController/WingController.h           | 24 ++++---
 .../VerticalVelocityTrigger.cpp               |  7 +-
 .../VerticalVelocityTrigger.h                 |  4 +-
 .../WindEstimationScheme/WindEstimation.cpp   | 12 ++--
 .../WindEstimationScheme/WindEstimation.h     |  5 +-
 src/entrypoints/Payload/payload-entry.cpp     | 57 +++++++---------
 24 files changed, 145 insertions(+), 213 deletions(-)
 delete mode 100644 src/boards/Payload/BoardScheduler.cpp

diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index ab4872b2a..f15b5c01b 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -83,7 +83,6 @@ set(PAYLOAD_COMPUTER
     src/boards/Payload/CanHandler/CanHandler.cpp
     src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
     src/boards/Payload/Sensors/Sensors.cpp
-    src/boards/Payload/BoardScheduler.cpp
     src/boards/Payload/PinHandler/PinHandler.cpp
     src/boards/Payload/Radio/Radio.cpp
     src/boards/Payload/TMRepository/TMRepository.cpp
diff --git a/src/boards/Payload/Actuators/Actuators.cpp b/src/boards/Payload/Actuators/Actuators.cpp
index 759aef675..ab96acfc4 100644
--- a/src/boards/Payload/Actuators/Actuators.cpp
+++ b/src/boards/Payload/Actuators/Actuators.cpp
@@ -35,7 +35,7 @@ using namespace Payload::ActuatorsConfigs;
 namespace Payload
 {
 
-Actuators::Actuators(Boardcore::TaskScheduler* sched) : scheduler(sched)
+Actuators::Actuators(Boardcore::TaskScheduler& sched) : scheduler(sched)
 {
     leftServo  = new Servo(SERVO_1_TIMER, SERVO_1_PWM_CH, LEFT_SERVO_MIN_PULSE,
                            LEFT_SERVO_MAX_PULSE);
@@ -56,8 +56,8 @@ bool Actuators::start()
 
     // Signaling Devices configurations
 
-    return scheduler->addTask([&]() { updateBuzzer(); }, BUZZER_UPDATE_PERIOD,
-                              TaskScheduler::Policy::RECOVER) != 0;
+    return scheduler.addTask([this]() { updateBuzzer(); }, BUZZER_UPDATE_PERIOD,
+                             TaskScheduler::Policy::RECOVER) != 0;
 }
 
 bool Actuators::setServo(ServosList servoId, float percentage)
diff --git a/src/boards/Payload/Actuators/Actuators.h b/src/boards/Payload/Actuators/Actuators.h
index 73a207d73..7a29bab90 100644
--- a/src/boards/Payload/Actuators/Actuators.h
+++ b/src/boards/Payload/Actuators/Actuators.h
@@ -34,7 +34,7 @@ namespace Payload
 
 struct Actuators : public Boardcore::Module
 {
-    explicit Actuators(Boardcore::TaskScheduler* sched);
+    explicit Actuators(Boardcore::TaskScheduler& sched);
 
     [[nodiscard]] bool start();
 
@@ -119,10 +119,10 @@ private:
      */
     void updateBuzzer();
 
-    Boardcore::TaskScheduler* scheduler = nullptr;
-    Boardcore::Servo* leftServo         = nullptr;
-    Boardcore::Servo* rightServo        = nullptr;
-    Boardcore::PWM* buzzer              = nullptr;
+    Boardcore::TaskScheduler& scheduler;
+    Boardcore::Servo* leftServo  = nullptr;
+    Boardcore::Servo* rightServo = nullptr;
+    Boardcore::PWM* buzzer       = nullptr;
 
     // mutexes
     miosix::FastMutex leftServoMutex;
diff --git a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
index 6398f6e51..d72a2c469 100644
--- a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
+++ b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp
@@ -34,7 +34,7 @@ using namespace Common;
 namespace Payload
 {
 
-AltitudeTrigger::AltitudeTrigger(TaskScheduler *sched)
+AltitudeTrigger::AltitudeTrigger(TaskScheduler& sched)
     : running(false), confidence(0),
       deploymentAltitude(WingConfig::ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE),
       scheduler(sched)
@@ -43,8 +43,8 @@ AltitudeTrigger::AltitudeTrigger(TaskScheduler *sched)
 
 bool AltitudeTrigger::start()
 {
-    return (scheduler->addTask(std::bind(&AltitudeTrigger::update, this),
-                               WingConfig::ALTITUDE_TRIGGER_PERIOD) != 0);
+    return (scheduler.addTask([this] { update(); },
+                              WingConfig::ALTITUDE_TRIGGER_PERIOD) != 0);
 }
 
 void AltitudeTrigger::enable()
diff --git a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h
index b0d2731ac..345406421 100644
--- a/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h
+++ b/src/boards/Payload/AltitudeTrigger/AltitudeTrigger.h
@@ -33,7 +33,7 @@ namespace Payload
 class AltitudeTrigger : public Boardcore::Module
 {
 public:
-    explicit AltitudeTrigger(Boardcore::TaskScheduler *sched);
+    explicit AltitudeTrigger(Boardcore::TaskScheduler &sched);
 
     /**
      * @brief Adds the update() task to the task scheduler.
@@ -61,21 +61,19 @@ public:
     void setDeploymentAltitude(float altitude);
 
 private:
-    // Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
-    // altitude is reached
+    /**
+     * @brief Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
+     * altitude is reached
+     */
     void update();
 
     bool running;
-
-    // Number of times that the algorithm detects to be below the fixed
-    // altitude
-    int confidence;
-
+    int confidence;  ///< Number of times that the algorithm detects to be below
+                     ///< the fixed altitude
     float deploymentAltitude;
 
     miosix::FastMutex mutex;
-
-    Boardcore::TaskScheduler *scheduler = nullptr;
+    Boardcore::TaskScheduler &scheduler;
 };
 
 }  // namespace Payload
diff --git a/src/boards/Payload/BoardScheduler.cpp b/src/boards/Payload/BoardScheduler.cpp
deleted file mode 100644
index 10eafa5f5..000000000
--- a/src/boards/Payload/BoardScheduler.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "BoardScheduler.h"
-
-using namespace Boardcore;
-
-namespace Payload
-{
-
-BoardScheduler::BoardScheduler()
-{
-    scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
-    scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
-    scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
-    scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
-}
-
-TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
-{
-    switch (priority.get())
-    {
-        case miosix::PRIORITY_MAX:
-            return scheduler4;
-        case miosix::PRIORITY_MAX - 1:
-            return scheduler3;
-        case miosix::PRIORITY_MAX - 2:
-            return scheduler2;
-        case miosix::MAIN_PRIORITY:
-            return scheduler1;
-        default:
-            return scheduler1;
-    }
-}
-
-bool BoardScheduler::start()
-{
-    return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
-           scheduler4->start();
-}
-
-bool BoardScheduler::isStarted()
-{
-    return scheduler1->isRunning() && scheduler2->isRunning() &&
-           scheduler3->isRunning() && scheduler4->isRunning();
-}
-}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h
index 3a09bccc3..9ec1a1d0e 100644
--- a/src/boards/Payload/BoardScheduler.h
+++ b/src/boards/Payload/BoardScheduler.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -34,29 +34,48 @@ namespace Payload
 class BoardScheduler : public Boardcore::Module
 {
 public:
-    BoardScheduler();
+    /**
+     * @brief Returns the scheduler for critical tasks (highest priority)
+     */
+    Boardcore::TaskScheduler& getCriticalScheduler() { return critical; }
+
+    /**
+     * @brief Returns the scheduler for high priority tasks
+     */
+    Boardcore::TaskScheduler& getHighScheduler() { return high; }
 
     /**
-     * @brief Get the Scheduler object relative to the requested priority
-     *
-     * @param priority The task scheduler priority
-     * @return Boardcore::TaskScheduler& Reference to the requested task
-     * scheduler.
-     * @note Min priority scheduler is returned in case of non valid priority.
+     * @brief Returns the scheduler for medium priority tasks
      */
-    Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+    Boardcore::TaskScheduler& getMediumScheduler() { return medium; }
 
-    [[nodiscard]] bool start();
+    /**
+     * @brief Returns the scheduler for low priority tasks (lowest priority)
+     */
+    Boardcore::TaskScheduler& getLowScheduler() { return low; }
+
+    /**
+     * @brief Starts all the schedulers
+     */
+    [[nodiscard]] bool start()
+    {
+        return critical.start() && high.start() && medium.start() &&
+               low.start();
+    }
 
     /**
      * @brief Returns if all the schedulers are up and running
      */
-    bool isStarted();
+    bool isStarted()
+    {
+        return critical.isRunning() && high.isRunning() && medium.isRunning() &&
+               low.isRunning();
+    }
 
 private:
-    Boardcore::TaskScheduler* scheduler1;
-    Boardcore::TaskScheduler* scheduler2;
-    Boardcore::TaskScheduler* scheduler3;
-    Boardcore::TaskScheduler* scheduler4;
+    Boardcore::TaskScheduler critical{miosix::PRIORITY_MAX - 1};
+    Boardcore::TaskScheduler high{miosix::PRIORITY_MAX - 2};
+    Boardcore::TaskScheduler medium{miosix::PRIORITY_MAX - 3};
+    Boardcore::TaskScheduler low{miosix::PRIORITY_MAX - 4};
 };
 }  // namespace Payload
diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp
index e9ba14e44..d23389e73 100644
--- a/src/boards/Payload/CanHandler/CanHandler.cpp
+++ b/src/boards/Payload/CanHandler/CanHandler.cpp
@@ -30,8 +30,6 @@
 
 #include <functional>
 
-using namespace std;
-using namespace placeholders;
 using namespace Boardcore;
 using namespace Canbus;
 using namespace Common;
@@ -41,17 +39,18 @@ using namespace Payload::CanHandlerConfig;
 namespace Payload
 {
 
-CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+CanHandler::CanHandler(TaskScheduler& sched) : scheduler(sched)
 {
     CanbusDriver::AutoBitTiming bitTiming;
     bitTiming.baudRate    = BAUD_RATE;
     bitTiming.samplePoint = SAMPLE_POINT;
+
     CanbusDriver::CanbusConfig config;
     driver = new CanbusDriver(CAN2, config, bitTiming);
 
-    protocol =
-        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
-                        miosix::PRIORITY_MAX - 1);
+    protocol = new CanProtocol(
+        driver, [this](auto msg) { handleCanMessage(msg); },
+        miosix::PRIORITY_MAX - 1);
 
     // Accept messages only from the main and RIG board
     protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
@@ -65,7 +64,7 @@ bool CanHandler::start()
 {
     bool result;
     // Add a task to periodically send the pitot data
-    result = scheduler->addTask(  // sensor template
+    result = scheduler.addTask(  // sensor template
                  [&]()
                  {
                      protocol->enqueueData(
@@ -82,7 +81,7 @@ bool CanHandler::start()
 
     result =
         result &&
-        scheduler->addTask(  // status
+        scheduler.addTask(  // status
             [&]()
             {
                 FlightModeManagerState state = ModuleManager::getInstance()
@@ -112,7 +111,7 @@ void CanHandler::sendEvent(EventId event)
                            static_cast<uint8_t>(event));
 }
 
-void CanHandler::handleCanMessage(const CanMessage &msg)
+void CanHandler::handleCanMessage(const CanMessage& msg)
 {
     PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
 
@@ -132,7 +131,7 @@ void CanHandler::handleCanMessage(const CanMessage &msg)
     }
 }
 
-void CanHandler::handleCanEvent(const Boardcore::Canbus::CanMessage &msg)
+void CanHandler::handleCanEvent(const Boardcore::Canbus::CanMessage& msg)
 {
     EventId eventId = static_cast<EventId>(msg.getSecondaryType());
 
diff --git a/src/boards/Payload/CanHandler/CanHandler.h b/src/boards/Payload/CanHandler/CanHandler.h
index 2f040b4e0..1e8a586e6 100644
--- a/src/boards/Payload/CanHandler/CanHandler.h
+++ b/src/boards/Payload/CanHandler/CanHandler.h
@@ -46,7 +46,7 @@ public:
      * @warning With Payload motherboard CanDriver initialization could be
      * blocking
      */
-    explicit CanHandler(Boardcore::TaskScheduler *sched);
+    explicit CanHandler(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Starts the protocol and adds the periodic messages to the task
@@ -72,19 +72,19 @@ private:
     /**
      * @brief Function called by CanProtocol when a new message is received
      */
-    void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanMessage(const Boardcore::Canbus::CanMessage& msg);
 
     /**
      * @brief Converts the received CanEvent in Event and post it on TOPIC_CAN
      */
-    void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanEvent(const Boardcore::Canbus::CanMessage& msg);
 
-    Boardcore::Canbus::CanbusDriver *driver;
-    Boardcore::Canbus::CanProtocol *protocol;
+    Boardcore::Canbus::CanbusDriver* driver;
+    Boardcore::Canbus::CanProtocol* protocol;
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
 
-    Boardcore::TaskScheduler *scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 };
 
 }  // namespace Payload
diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
index faf62811c..8e697720b 100644
--- a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
+++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -35,7 +35,7 @@ using namespace miosix;
 
 namespace Payload
 {
-FlightStatsRecorder::FlightStatsRecorder(TaskScheduler* sched)
+FlightStatsRecorder::FlightStatsRecorder(TaskScheduler& sched)
     : scheduler(sched)
 {
     // Init the data structure to avoid UB
@@ -58,8 +58,8 @@ FlightStatsRecorder::FlightStatsRecorder(TaskScheduler* sched)
 
 bool FlightStatsRecorder::start()
 {
-    return scheduler->addTask([&]() { update(); }, FSR_UPDATE_PERIOD,
-                              TaskScheduler::Policy::RECOVER);
+    return scheduler.addTask([this] { update(); }, FSR_UPDATE_PERIOD,
+                             TaskScheduler::Policy::RECOVER);
 }
 
 void FlightStatsRecorder::update()
diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h
index 2b034bd4a..8041652b1 100644
--- a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h
+++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.h
@@ -37,7 +37,7 @@ namespace Payload
 class FlightStatsRecorder : public Boardcore::Module
 {
 public:
-    FlightStatsRecorder(Boardcore::TaskScheduler* sched);
+    explicit FlightStatsRecorder(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Adds a task to the scheduler to update the stats
@@ -69,7 +69,7 @@ private:
                    Boardcore::NASState data);
 
     // Update scheduler to update all the data
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 
     // Data structure
     mavlink_payload_stats_tm_t stats;
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 3611f2a44..392f38deb 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -73,7 +73,7 @@ void __attribute__((used)) SX1278_IRQ_DIO3()
 namespace Payload
 {
 
-Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
+Radio::Radio(TaskScheduler& sched) : scheduler(sched) {}
 
 bool Radio::start()
 {
@@ -105,10 +105,10 @@ bool Radio::start()
 
     // Add periodic telemetry send task
     uint8_t result =
-        scheduler->addTask([&]() { this->sendPeriodicMessage(); },
-                           RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
-                           TaskScheduler::Policy::RECOVER);
-    result *= scheduler->addTask(
+        scheduler.addTask([&]() { this->sendPeriodicMessage(); },
+                          RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
+                          TaskScheduler::Policy::RECOVER);
+    result *= scheduler.addTask(
         [&]()
         {
             this->enqueueMsg(
@@ -119,8 +119,7 @@ bool Radio::start()
 
     // Config mavDriver
     mavDriver = new MavDriver(
-        transceiver,
-        [=](MavDriver*, const mavlink_message_t& msg)
+        transceiver, [=](MavDriver*, const mavlink_message_t& msg)
         { this->handleMavlinkMessage(msg); },
         RadioConfig::RADIO_SLEEP_AFTER_SEND,
         RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
@@ -157,7 +156,7 @@ void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); }
 
 bool Radio::isStarted()
 {
-    return mavDriver->isStarted() && scheduler->isRunning();
+    return mavDriver->isStarted() && scheduler.isRunning();
 }
 
 void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h
index 10cb47178..c5fdc2489 100644
--- a/src/boards/Payload/Radio/Radio.h
+++ b/src/boards/Payload/Radio/Radio.h
@@ -38,7 +38,7 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
 class Radio : public Boardcore::Module
 {
 public:
-    Radio(Boardcore::TaskScheduler* sched);
+    Radio(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Starts the MavlinkDriver
@@ -97,7 +97,7 @@ private:
     miosix::FastMutex queueMutex;
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 };
 
 }  // namespace Payload
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index d43d378e2..ecc7fd213 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -165,7 +165,7 @@ MagnetometerData Sensors::getCalibratedMagnetometerLastSample()
     return result;
 }
 
-Sensors::Sensors(TaskScheduler* sched) : scheduler(sched), sensorsCounter(0) {}
+Sensors::Sensors(TaskScheduler& sched) : scheduler(sched), sensorsCounter(0) {}
 
 bool Sensors::start()
 {
@@ -186,7 +186,7 @@ bool Sensors::start()
     imuInit();
 
     // Add the magnetometer calibration to the scheduler
-    size_t result = scheduler->addTask(
+    size_t result = scheduler.addTask(
         [&]()
         {
             // Gather the last sample data
@@ -203,7 +203,7 @@ bool Sensors::start()
         MAG_CALIBRATION_PERIOD);
 
     // Create sensor manager with populated map and configured scheduler
-    manager = new SensorManager(sensorMap, scheduler);
+    manager = new SensorManager(sensorMap, &scheduler);
     return manager->start() && result != 0;
 }
 
@@ -211,7 +211,7 @@ void Sensors::stop() { manager->stop(); }
 
 bool Sensors::isStarted()
 {
-    return manager->areAllSensorsInitialized() && scheduler->isRunning();
+    return manager->areAllSensorsInitialized() && scheduler.isRunning();
 }
 
 void Sensors::calibrate()
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index faeee0b84..62ce45051 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -49,7 +49,7 @@ namespace Payload
 class Sensors : public Boardcore::Module
 {
 public:
-    explicit Sensors(Boardcore::TaskScheduler* sched);
+    explicit Sensors(Boardcore::TaskScheduler& sched);
 
     [[nodiscard]] bool start();
 
@@ -164,7 +164,7 @@ private:
     // Sensor manager
     Boardcore::SensorManager* manager = nullptr;
     Boardcore::SensorManager::SensorMap_t sensorMap;
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 
     uint8_t sensorsCounter;
 
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index 8bb088495..b68152a70 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -31,11 +31,11 @@
 
 using namespace Boardcore;
 using namespace Eigen;
-using namespace std;
 using namespace Common;
+
 namespace Payload
 {
-NASController::NASController(TaskScheduler* sched)
+NASController::NASController(TaskScheduler& sched)
     : FSM(&NASController::state_idle), nas(NASConfig::config), scheduler(sched)
 {
     // Subscribe the class to the topics
@@ -64,9 +64,9 @@ NASController::NASController(TaskScheduler* sched)
 bool NASController::start()
 {
     // Add the task to the scheduler
-    size_t result = scheduler->addTask(bind(&NASController::update, this),
-                                       NASConfig::UPDATE_PERIOD,
-                                       TaskScheduler::Policy::RECOVER);
+    size_t result =
+        scheduler.addTask([this] { update(); }, NASConfig::UPDATE_PERIOD,
+                          TaskScheduler::Policy::RECOVER);
 
     return ActiveObject::start() && result != 0;
 }
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h
index 73a256714..3cf42f15a 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.h
+++ b/src/boards/Payload/StateMachines/NASController/NASController.h
@@ -37,7 +37,7 @@ class NASController : public Boardcore::FSM<NASController>,
                       public Boardcore::Module
 {
 public:
-    NASController(Boardcore::TaskScheduler* sched);
+    NASController(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Starts the FSM thread and adds the update function into the
@@ -84,7 +84,7 @@ private:
     Boardcore::NAS nas;
 
     // Scheduler to be used for update function
-    Boardcore::TaskScheduler* scheduler;
+    Boardcore::TaskScheduler& scheduler;
 
     // User set (or triac set) initial orientation
     Eigen::Vector3f initialOrientation;
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 33a64f24a..fa753f2c8 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -47,7 +47,7 @@ using namespace miosix;
 namespace Payload
 {
 
-WingController::WingController(TaskScheduler* sched)
+WingController::WingController(TaskScheduler& sched)
     : HSM(&WingController::state_idle), running(false), selectedAlgorithm(0),
       scheduler(sched)
 {
@@ -59,8 +59,7 @@ WingController::WingController(TaskScheduler* sched)
 
 bool WingController::start()
 {
-    return scheduler->addTask(std::bind(&WingController::update, this),
-                              WING_UPDATE_PERIOD) &&
+    return scheduler.addTask([this] { update(); }, WING_UPDATE_PERIOD) &&
            addAlgorithms() && HSM::start();
 }
 
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 1fb84ee66..e2fee5eaf 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -57,21 +57,24 @@ namespace Payload
 {
 class WingController : public Boardcore::HSM<WingController>,
                        public Boardcore::Module
-
 {
-
 public:
-    Boardcore::State state_idle(const Boardcore::Event& event);
-    Boardcore::State state_flying(const Boardcore::Event& event);
-    Boardcore::State state_calibration(const Boardcore::Event& event);
-    Boardcore::State state_controlled_descent(const Boardcore::Event& event);
-    Boardcore::State state_on_ground(const Boardcore::Event& event);
+    /**
+     * @brief Construct a new Wing Controller object
+     */
+    explicit WingController(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Destroy the Wing Controller object.
      */
     ~WingController();
 
+    Boardcore::State state_idle(const Boardcore::Event& event);
+    Boardcore::State state_flying(const Boardcore::Event& event);
+    Boardcore::State state_calibration(const Boardcore::Event& event);
+    Boardcore::State state_controlled_descent(const Boardcore::Event& event);
+    Boardcore::State state_on_ground(const Boardcore::Event& event);
+
     /**
      * @brief Method to set the target position.
      */
@@ -88,11 +91,6 @@ public:
 
     WingControllerStatus getStatus();
 
-    /**
-     * @brief Construct a new Wing Controller object
-     */
-    explicit WingController(Boardcore::TaskScheduler* sched);
-
     bool start() override;
 
 private:
@@ -177,6 +175,6 @@ private:
 
     void update();
 
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 };
 }  // namespace Payload
diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
index 538d58a45..f99f176f6 100644
--- a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
+++ b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.cpp
@@ -29,22 +29,21 @@
 
 #include <functional>
 
-using namespace std;
 using namespace Boardcore;
 using namespace Common;
 
 namespace Payload
 {
 
-VerticalVelocityTrigger::VerticalVelocityTrigger(TaskScheduler* sched)
+VerticalVelocityTrigger::VerticalVelocityTrigger(TaskScheduler& sched)
     : running(false), confidence(0), scheduler(sched)
 {
 }
 
 bool VerticalVelocityTrigger::start()
 {
-    return scheduler->addTask(
-        bind(&VerticalVelocityTrigger::update, this),
+    return scheduler.addTask(
+        [this] { update(); },
         FailSafe::FAILSAFE_VERTICAL_VELOCITY_TRIGGER_PERIOD);
 }
 
diff --git a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h
index 84f721f35..a0c137c53 100644
--- a/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h
+++ b/src/boards/Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h
@@ -44,7 +44,7 @@ public:
      * Default constructor for VerticalVelocityTrigger
      * Sets the trigger to disabled by default
      */
-    VerticalVelocityTrigger(Boardcore::TaskScheduler* sched);
+    explicit VerticalVelocityTrigger(Boardcore::TaskScheduler& sched);
 
     /**
      * Starts the module by inserting it in the BoardScheduler
@@ -84,7 +84,7 @@ private:
     // threshold
     std::atomic<int> confidence;
 
-    Boardcore::TaskScheduler* scheduler;
+    Boardcore::TaskScheduler& scheduler;
 };
 
 }  // namespace Payload
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
index e6cc50cc7..f705cbc38 100644
--- a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp
@@ -33,12 +33,11 @@
 using namespace Payload::WESConfig;
 using namespace Boardcore;
 using namespace Common;
-using namespace miosix;
 
 namespace Payload
 {
 
-WindEstimation::WindEstimation(TaskScheduler* sched)
+WindEstimation::WindEstimation(TaskScheduler& sched)
     : running(false), calRunning(false), scheduler(sched)
 {
     funv << 1.0f, 0.0f, 0.0f, 1.0f;  // cppcheck-suppress constStatement
@@ -46,13 +45,12 @@ WindEstimation::WindEstimation(TaskScheduler* sched)
 
 bool WindEstimation::start()
 {
-    scheduler->addTask(
-        std::bind(&WindEstimation::windEstimationSchemeCalibration, this),
-        WES_CALIBRATION_UPDATE_PERIOD);
+    scheduler.addTask([this] { windEstimationSchemeCalibration(); },
+                      WES_CALIBRATION_UPDATE_PERIOD);
 
     // Register the WES task
-    scheduler->addTask(std::bind(&WindEstimation::windEstimationScheme, this),
-                       WES_PREDICTION_UPDATE_PERIOD);
+    scheduler.addTask([this] { windEstimationScheme(); },
+                      WES_PREDICTION_UPDATE_PERIOD);
 
     return true;
 }
diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.h b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
index e3c5e0148..b54feba6b 100644
--- a/src/boards/Payload/WindEstimationScheme/WindEstimation.h
+++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.h
@@ -32,6 +32,7 @@
 #include <utils/ModuleManager/ModuleManager.hpp>
 
 #include "WindEstimationData.h"
+
 namespace Payload
 {
 
@@ -45,7 +46,7 @@ public:
     /**
      * @brief Construct a new Wing Controller object
      */
-    explicit WindEstimation(Boardcore::TaskScheduler* sched);
+    explicit WindEstimation(Boardcore::TaskScheduler& sched);
 
     /**
      * @brief Destroy the Wing Controller object.
@@ -125,6 +126,6 @@ private:
     std::atomic<bool> running;
     std::atomic<bool> calRunning;
 
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 };
 }  // namespace Payload
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index 00bdfcda5..0554b34d7 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -61,44 +61,33 @@ int main()
     bool initResult    = true;
     PrintLogger logger = Logging::getLogger("Payload");
 
-    // Scheduler
-    BoardScheduler* scheduler = new BoardScheduler();
-
-    // Nas priority (Max priority)
-    NASController* nas =
-        new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX));
-
-    // Sensors priority (MAX - 1)
-    Sensors* sensors =
-        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
-
-    // Other critical components (Max - 2)
-    Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    AltitudeTrigger* altTrigger =
-        new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    WingController* wingController =
-        new WingController(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    VerticalVelocityTrigger* verticalVelocityTrigger =
-        new VerticalVelocityTrigger(
-            scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    WindEstimation* windEstimation =
-        new WindEstimation(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    CanHandler* canHandler =
-        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-
-    // Non critical components (Max - 3)
+    auto buses     = new Buses();
+    auto scheduler = new BoardScheduler();
+
+    // Attitude estimation are critical components
+    auto nas     = new NASController(scheduler->getCriticalScheduler());
+    auto sensors = new Sensors(scheduler->getHighScheduler());
+
+    // Radio and CAN
+    auto radio      = new Radio(scheduler->getMediumScheduler());
+    auto canHandler = new CanHandler(scheduler->getMediumScheduler());
+
+    // Flight algorithms
+    auto altTrigger     = new AltitudeTrigger(scheduler->getMediumScheduler());
+    auto wingController = new WingController(scheduler->getMediumScheduler());
+    auto verticalVelocityTrigger =
+        new VerticalVelocityTrigger(scheduler->getMediumScheduler());
+    auto windEstimation = new WindEstimation(scheduler->getMediumScheduler());
+
     // Actuators is considered non-critical since the scheduler is only used for
     // the led and buzzer tasks
-    Actuators* actuators =
-        new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX - 3));
-    FlightStatsRecorder* statesRecorder = new FlightStatsRecorder(
-        scheduler->getScheduler(miosix::PRIORITY_MAX - 3));
+    auto actuators      = new Actuators(scheduler->getLowScheduler());
+    auto statesRecorder = new FlightStatsRecorder(scheduler->getLowScheduler());
 
     // Components without a scheduler
-    TMRepository* tmRepo   = new TMRepository();
-    FlightModeManager* fmm = new FlightModeManager();
-    Buses* buses           = new Buses();
-    PinHandler* pinHandler = new PinHandler();
+    auto tmRepo     = new TMRepository();
+    auto fmm        = new FlightModeManager();
+    auto pinHandler = new PinHandler();
 
     // Insert modules
     if (!modules.insert<BoardScheduler>(scheduler))
-- 
GitLab