From 1560b06284156a07c6bdd6973efa6d40b55b3773 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 19 Jul 2024 12:30:17 +0200
Subject: [PATCH] [RIGv2] Finalized CanHandler

---
 src/boards/RIGv2/Actuators/Actuators.cpp    |  10 +-
 src/boards/RIGv2/Actuators/Actuators.h      |   6 +-
 src/boards/RIGv2/CanHandler/CanHandler.cpp  |  93 ++++++-----
 src/boards/RIGv2/CanHandler/CanHandler.h    |  12 +-
 src/boards/RIGv2/Configs/CanHandlerConfig.h |   4 +-
 src/boards/RIGv2/Configs/SensorsConfig.h    |  20 +--
 src/boards/RIGv2/Radio/Radio.cpp            |  18 +-
 src/boards/RIGv2/Sensors/Sensors.cpp        | 174 +++++++++++---------
 src/boards/RIGv2/Sensors/Sensors.h          |  20 ++-
 9 files changed, 195 insertions(+), 162 deletions(-)

diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 012ceed80..7bb56582a 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -400,11 +400,11 @@ bool Actuators::isCanServoOpen(ServosList servo)
     Lock<FastMutex> lock(infosMutex);
     if (servo == ServosList::MAIN_VALVE)
     {
-        return mainAperture > 0.05f;
+        return canMainOpen;
     }
     else if (servo == ServosList::VENTING_VALVE)
     {
-        return ventingAperture > 0.05f;
+        return canVentingOpen;
     }
     else
     {
@@ -462,16 +462,16 @@ float Actuators::getServoMaxAperture(ServosList servo)
     return info->getMaxAperture();
 }
 
-void Actuators::setCanServoAperture(ServosList servo, float aperture)
+void Actuators::setCanServoOpen(ServosList servo, bool open)
 {
     Lock<FastMutex> lock(infosMutex);
     if (servo == ServosList::MAIN_VALVE)
     {
-        mainAperture = aperture;
+        canMainOpen = open;
     }
     else if (servo == ServosList::VENTING_VALVE)
     {
-        ventingAperture = aperture;
+        canVentingOpen = open;
     }
 }
 
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index 630a06be5..bd2ef33fd 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -106,7 +106,7 @@ public:
     void igniterOn();
     void igniterOff();
 
-    void setCanServoAperture(ServosList servo, float aperture);
+    void setCanServoOpen(ServosList servo, bool open);
 
     void inject(Boardcore::DependencyInjector &injector) override;
 
@@ -131,8 +131,8 @@ private:
     // Timestamp of last servo action (open/close)
     long long nitrogenLastActionTs = 0;
 
-    float mainAperture    = 0.0f;
-    float ventingAperture = 0.0f;
+    bool canMainOpen    = false;
+    bool canVentingOpen = false;
 };
 
 }  // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/CanHandler/CanHandler.cpp b/src/boards/RIGv2/CanHandler/CanHandler.cpp
index 6b2a9d7dd..d8c34cbe6 100644
--- a/src/boards/RIGv2/CanHandler/CanHandler.cpp
+++ b/src/boards/RIGv2/CanHandler/CanHandler.cpp
@@ -25,6 +25,8 @@
 #include <RIGv2/Actuators/Actuators.h>
 #include <RIGv2/Configs/SchedulerConfig.h>
 #include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <common/CanConfig.h>
+#include <drivers/timer/TimestampTimer.h>
 
 using namespace miosix;
 using namespace RIGv2;
@@ -32,12 +34,8 @@ using namespace Boardcore;
 using namespace Boardcore::Canbus;
 using namespace Common;
 
-static constexpr CanbusDriver::CanbusConfig CONFIG;
-static constexpr CanbusDriver::AutoBitTiming BIT_TIMING = {
-    .baudRate = CanConfig::BAUD_RATE, .samplePoint = CanConfig::SAMPLE_POINT};
-
 CanHandler::CanHandler()
-    : driver(CAN1, CONFIG, BIT_TIMING),
+    : driver(CAN1, CanConfig::CONFIG, CanConfig::BIT_TIMING),
       protocol(
           &driver, [this](const CanMessage &msg) { handleMessage(msg); },
           Config::Scheduler::CAN_PRIORITY)
@@ -59,10 +57,31 @@ bool CanHandler::start()
     TaskScheduler &scheduler =
         getModule<BoardScheduler>()->getCanBusScheduler();
 
-    uint8_t result = scheduler.addTask([this]() { periodicMessage(); },
-                                       Config::CanHandler::STATUS_PERIOD);
-
-    if (result != 0)
+    uint8_t result = scheduler.addTask(
+        [this]()
+        {
+            LoggerStats stats = sdLogger.getStats();
+
+            GroundModeManagerState state =
+                getModule<GroundModeManager>()->getState();
+
+            protocol.enqueueData(
+                static_cast<uint8_t>(CanConfig::Priority::MEDIUM),
+                static_cast<uint8_t>(CanConfig::PrimaryType::STATUS),
+                static_cast<uint8_t>(CanConfig::Board::RIG),
+                static_cast<uint8_t>(CanConfig::Board::BROADCAST), 0x00,
+                DeviceStatus{
+                    TimestampTimer::getTimestamp(),
+                    static_cast<int16_t>(stats.logNumber),
+                    static_cast<uint8_t>(state),
+                    state == GroundModeManagerState::GMM_STATE_ARMED,
+                    false,
+                    stats.lastWriteError == 0,
+                });
+        },
+        Config::CanHandler::STATUS_PERIOD);
+
+    if (result == 0)
     {
         LOG_ERR(logger, "Failed to add periodicMessageTask");
         return false;
@@ -88,20 +107,16 @@ void CanHandler::sendEvent(CanConfig::EventId event)
 }
 
 void CanHandler::sendServoOpenCommand(ServosList servo, float maxAperture,
-                                      uint32_t openingTime)
+                                      uint16_t openingTime)
 {
-    uint32_t maxApertureBits = 0;
-    std::memcpy(&maxApertureBits, &maxAperture, sizeof(maxAperture));
-
-    uint64_t payload = (static_cast<uint64_t>(openingTime) << 32) |
-                       static_cast<uint64_t>(maxApertureBits);
 
-    protocol.enqueueSimplePacket(
+    protocol.enqueueData(
         static_cast<uint8_t>(CanConfig::Priority::CRITICAL),
         static_cast<uint8_t>(CanConfig::PrimaryType::COMMAND),
         static_cast<uint8_t>(CanConfig::Board::RIG),
         static_cast<uint8_t>(CanConfig::Board::BROADCAST),
-        static_cast<uint8_t>(servo), payload);
+        static_cast<uint8_t>(servo),
+        ServoCommand{TimestampTimer::getTimestamp(), maxAperture, openingTime});
 }
 
 CanHandler::CanStatus CanHandler::getCanStatus()
@@ -219,47 +234,46 @@ void CanHandler::handleSensor(const Boardcore::Canbus::CanMessage &msg)
 
 void CanHandler::handleActuator(const Boardcore::Canbus::CanMessage &msg)
 {
-    ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
-    ServoData data   = servoDataFromCanMessage(msg);
+    ServosList servo      = static_cast<ServosList>(msg.getSecondaryType());
+    CanServoFeedback data = servoFeedbackFromCanMessage(msg);
+    sdLogger.log(data);
 
-    // TODO: Update with new message
-
-    getModule<Actuators>()->setCanServoAperture(servo, data.position);
+    getModule<Actuators>()->setCanServoOpen(servo, data.open);
 }
 
 void CanHandler::handleStatus(const Boardcore::Canbus::CanMessage &msg)
 {
     CanConfig::Board source = static_cast<CanConfig::Board>(msg.getSource());
-
-    bool armed    = (msg.payload[0] >> 8) != 0;
-    uint8_t state = msg.payload[0] & 0xff;
+    CanDeviceStatus deviceStatus = deviceStatusFromCanMessage(msg);
 
     Lock<FastMutex> lock{statusMutex};
 
-    // TODO: Update with new message
-
     switch (source)
     {
         case CanConfig::Board::MAIN:
         {
             status.mainLastStatus = getTime();
-            status.mainArmed      = armed;
-            status.mainState      = state;
+            status.mainArmed      = deviceStatus.armed;
+            status.mainState      = deviceStatus.state;
             break;
         }
 
         case CanConfig::Board::PAYLOAD:
         {
             status.payloadLastStatus = getTime();
-            status.payloadArmed      = armed;
-            status.payloadState      = state;
+            status.payloadArmed      = deviceStatus.armed;
+            status.payloadState      = deviceStatus.state;
             break;
         }
 
         case CanConfig::Board::MOTOR:
         {
             status.motorLastStatus = getTime();
-            status.motorState      = state;
+            status.motorState      = deviceStatus.state;
+
+            status.motorLogNumber = deviceStatus.logNumber;
+            status.motorLogGood   = deviceStatus.logGood;
+            status.motorHil       = deviceStatus.hil;
             break;
         }
 
@@ -268,19 +282,4 @@ void CanHandler::handleStatus(const Boardcore::Canbus::CanMessage &msg)
             LOG_WARN(logger, "Received unsupported status: {}", source);
         }
     }
-}
-
-void CanHandler::periodicMessage()
-{
-    GroundModeManagerState state = getModule<GroundModeManager>()->getState();
-
-    uint64_t payload =
-        static_cast<uint8_t>(state) |
-        ((state == GroundModeManagerState::GMM_STATE_ARMED) ? 1 : 0) << 8;
-
-    protocol.enqueueSimplePacket(
-        static_cast<uint8_t>(CanConfig::Priority::MEDIUM),
-        static_cast<uint8_t>(CanConfig::PrimaryType::STATUS),
-        static_cast<uint8_t>(CanConfig::Board::RIG),
-        static_cast<uint8_t>(CanConfig::Board::BROADCAST), 0x00, payload);
 }
\ No newline at end of file
diff --git a/src/boards/RIGv2/CanHandler/CanHandler.h b/src/boards/RIGv2/CanHandler/CanHandler.h
index 68f989d22..4085bb229 100644
--- a/src/boards/RIGv2/CanHandler/CanHandler.h
+++ b/src/boards/RIGv2/CanHandler/CanHandler.h
@@ -54,6 +54,10 @@ public:
         bool mainArmed    = false;
         bool payloadArmed = false;
 
+        uint16_t motorLogNumber = 0;
+        bool motorLogGood       = true;
+        bool motorHil           = false;
+
         bool isMainConnected()
         {
             return miosix::getTime() <=
@@ -79,6 +83,10 @@ public:
 
         bool isMainArmed() { return mainArmed; }
         bool isPayloadArmed() { return payloadArmed; }
+
+        uint16_t getMotorLogNumber() { return motorLogNumber; }
+        bool isMotorLogGood() { return motorLogGood; }
+        bool isMotorHil() { return motorHil; }
     };
 
     CanHandler();
@@ -90,7 +98,7 @@ public:
     void sendEvent(Common::CanConfig::EventId event);
 
     void sendServoOpenCommand(ServosList servo, float maxAperture,
-                              uint32_t openingTime);
+                              uint16_t openingTime);
     void sendServoCloseCommand(ServosList servo);
 
     CanStatus getCanStatus();
@@ -102,8 +110,6 @@ private:
     void handleActuator(const Boardcore::Canbus::CanMessage &msg);
     void handleStatus(const Boardcore::Canbus::CanMessage &msg);
 
-    void periodicMessage();
-
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
     Boardcore::Logger &sdLogger   = Boardcore::Logger::getInstance();
 
diff --git a/src/boards/RIGv2/Configs/CanHandlerConfig.h b/src/boards/RIGv2/Configs/CanHandlerConfig.h
index 3a700c4a7..df1d519a8 100644
--- a/src/boards/RIGv2/Configs/CanHandlerConfig.h
+++ b/src/boards/RIGv2/Configs/CanHandlerConfig.h
@@ -35,8 +35,8 @@ namespace CanHandler
 
 using namespace std::chrono_literals;
 
-static constexpr std::chrono::nanoseconds STATUS_PERIOD  = 2s;
-static constexpr std::chrono::nanoseconds STATUS_TIMEOUT = 3s;
+static constexpr std::chrono::nanoseconds STATUS_PERIOD  = 1000ms;
+static constexpr std::chrono::nanoseconds STATUS_TIMEOUT = 1500ms;
 
 }  // namespace CanHandler
 
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index b638ad116..3cbe1db21 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -43,6 +43,10 @@ namespace ADS131M08
 
 using namespace Boardcore::Units::Frequency;
 
+constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR =
+    Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+constexpr bool GLOBAL_CHOP_MODE_EN = true;
+
 constexpr float CH1_SHUNT_RESISTANCE = 29.4048;
 constexpr float CH2_SHUNT_RESISTANCE = 29.5830;
 constexpr float CH3_SHUNT_RESISTANCE = 29.4973;
@@ -75,8 +79,8 @@ constexpr Boardcore::ADS131M08Defs::Channel TANK_LC_CHANNEL =
 constexpr float SERVO_CURRENT_SCALE = 4.5466;
 constexpr float SERVO_CURRENT_ZERO  = 2.520 / SERVO_CURRENT_SCALE;
 
-constexpr Hertz SAMPLE_PERIOD = 100_hz;
-constexpr bool ENABLED        = true;
+constexpr Hertz PERIOD = 100_hz;
+constexpr bool ENABLED = true;
 }  // namespace ADS131M08
 
 namespace MAX31856
@@ -84,15 +88,12 @@ namespace MAX31856
 
 using namespace Boardcore::Units::Frequency;
 
-constexpr Hertz SAMPLE_PERIOD = 10_hz;
-constexpr bool ENABLED        = true;
+constexpr Hertz PERIOD = 10_hz;
+constexpr bool ENABLED = true;
 }  // namespace MAX31856
 
 namespace Trafag
 {
-
-using namespace Boardcore::Units::Frequency;
-
 constexpr float FILLING_SHUNT_RESISTANCE     = ADS131M08::CH2_SHUNT_RESISTANCE;
 constexpr float TANK_TOP_SHUNT_RESISTANCE    = ADS131M08::CH4_SHUNT_RESISTANCE;
 constexpr float TANK_BOTTOM_SHUNT_RESISTANCE = ADS131M08::CH3_SHUNT_RESISTANCE;
@@ -109,9 +110,6 @@ constexpr float VESSEL_MAX_PRESSURE      = 400;  // bar
 
 namespace LoadCell
 {
-
-using namespace Boardcore::Units::Frequency;
-
 constexpr unsigned int CALIBRATE_SAMPLE_COUNT  = 10;
 constexpr unsigned int CALIBRATE_SAMPLE_PERIOD = 40;
 
@@ -143,7 +141,7 @@ constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL =
     Boardcore::InternalADC::CH14;
 
 constexpr float BATTERY_VOLTAGE_SCALE = 4.7917;
-constexpr Hertz SAMPLE_PERIOD         = 10_hz;
+constexpr Hertz PERIOD                = 10_hz;
 constexpr bool ENABLED                = true;
 }  // namespace InternalADC
 
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 8dee36f3c..79e467324 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -559,8 +559,9 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
                 sensors->getUmbilicalCurrent().current;
 
             // Log data
-            tm.log_good   = 0;  // TODO
-            tm.log_number = 0;  // TODO
+            LoggerStats stats = sdLogger.getStats();
+            tm.log_number     = stats.logNumber;
+            tm.log_good       = stats.lastWriteError == 0;
 
             // Valve states
             tm.filling_valve_state =
@@ -615,18 +616,19 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
             tm.tank_temperature = sensors->getTc1LastSample().temperature;
             tm.battery_voltage  = sensors->getMotorBatteryVoltage().voltage;
 
-            // Log data
-            tm.log_good   = 0;  // TODO
-            tm.log_number = 0;  // TODO
-
             // Valve states
             tm.main_valve_state =
                 actuators->isCanServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
             tm.venting_valve_state =
                 actuators->isCanServoOpen(ServosList::VENTING_VALVE) ? 1 : 0;
 
-            // HIL metadata
-            tm.hil_state = 0;  // TODO
+            // Can data
+            CanHandler::CanStatus canStatus =
+                getModule<CanHandler>()->getCanStatus();
+
+            tm.log_number = canStatus.getMotorLogNumber();
+            tm.log_good   = canStatus.isMotorLogGood() ? 1 : 0;
+            tm.hil_state  = canStatus.isMotorHil() ? 1 : 0;
 
             mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
                                         Config::Radio::MAV_COMPONENT_ID, &msg,
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 806434052..a3ba2e20f 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -35,33 +35,30 @@ bool Sensors::isStarted() { return started; }
 
 bool Sensors::start()
 {
-    TaskScheduler &scheduler =
-        getModule<BoardScheduler>()->getSensorsScheduler();
-
-    SensorManager::SensorMap_t map;
     if (Config::Sensors::InternalADC::ENABLED)
     {
-        internalAdcInit(map);
+        internalAdcInit();
     }
+
     if (Config::Sensors::ADS131M08::ENABLED)
     {
-        adc1Init(map);
+        adc1Init();
+        vesselPressureInit();
+        fillingPressureInit();
+        topTankPressureInit();
+        bottomTankPressureInit();
+        vesselWeightInit();
+        tankWeightInit();
     }
+
     if (Config::Sensors::MAX31856::ENABLED)
     {
-        tc1Init(map);
+        tc1Init();
     }
-    vesselPressureInit(map);
-    fillingPressureInit(map);
-    topTankPressureInit(map);
-    bottomTankPressureInit(map);
-    vesselWeightInit(map);
-    tankWeightInit(map);
 
-    manager = std::make_unique<SensorManager>(map, &scheduler);
-    if (!manager->start())
+    if (!sensorManagerInit())
     {
-        LOG_ERR(logger, "Failed to start SensorManager");
+        LOG_ERR(logger, "Failed to init SensorManager");
         return false;
     }
 
@@ -289,7 +286,7 @@ std::vector<SensorInfo> Sensors::getSensorInfos()
     }
 }
 
-void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::internalAdcInit()
 {
     internalAdc = std::make_unique<InternalADC>(ADC1);
 
@@ -299,10 +296,6 @@ void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::InternalADC::BATTERY_VOLTAGE_CHANNEL);
     internalAdc->enableTemperature();
     internalAdc->enableVbat();
-
-    SensorInfo info("InternalAdc", Config::Sensors::InternalADC::SAMPLE_PERIOD,
-                    [this]() { internalAdcCallback(); });
-    map.emplace(internalAdc.get(), info);
 }
 
 void Sensors::internalAdcCallback()
@@ -311,7 +304,7 @@ void Sensors::internalAdcCallback()
     sdLogger.log(sample);
 }
 
-void Sensors::adc1Init(SensorManager::SensorMap_t &map)
+void Sensors::adc1Init()
 {
     SPIBusConfig spiConfig = {};
     spiConfig.mode         = SPI::Mode::MODE_0;
@@ -319,8 +312,9 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map)
 
     ADS131M08::Config config = {};
     // Setup global configurations
-    config.oversamplingRatio     = ADS131M08Defs::OversamplingRatio::OSR_8192;
-    config.globalChopModeEnabled = true;
+    config.oversamplingRatio = Config::Sensors::ADS131M08::OSR;
+    config.globalChopModeEnabled =
+        Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN;
 
     // Disable all channels
     config.channelsConfig[0].enabled = false;
@@ -379,10 +373,6 @@ void Sensors::adc1Init(SensorManager::SensorMap_t &map)
     adc1 = std::make_unique<ADS131M08>(getModule<Buses>()->getADS131M08_1(),
                                        sensors::ADS131_1::cs::getPin(),
                                        spiConfig, config);
-
-    SensorInfo info("ADS131M08_1", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { adc1Callback(); });
-    map.emplace(std::make_pair(adc1.get(), info));
 }
 
 void Sensors::adc1Callback()
@@ -395,18 +385,10 @@ void Sensors::adc1Callback()
                   sample.voltage[4], sample.voltage[5],
                   sample.voltage[6], sample.voltage[7]};
 
-    // LOG_INFO(logger, "{:.4}\t{:.4}\t{:.4}\t{:.4}",
-    //          (sample.voltage[0] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE)
-    //          * 1000.0f, (sample.voltage[1] /
-    //          Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE) * 1000.0f,
-    //          (sample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE)
-    //          * 1000.0f, (sample.voltage[3] /
-    //          Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE) * 1000.0f);
-
     sdLogger.log(data);
 }
 
-void Sensors::tc1Init(SensorManager::SensorMap_t &map)
+void Sensors::tc1Init()
 {
     SPIBusConfig spiConfig = MAX31856::getDefaultSPIConfig();
     spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
@@ -414,10 +396,6 @@ void Sensors::tc1Init(SensorManager::SensorMap_t &map)
     tc1 = std::make_unique<MAX31856>(
         getModule<Buses>()->getMAX31856_1(), sensors::MAX31856_1::cs::getPin(),
         spiConfig, MAX31856::ThermocoupleType::K_TYPE);
-
-    SensorInfo info("MAX31856_1", Config::Sensors::MAX31856::SAMPLE_PERIOD,
-                    [this]() { tc1Callback(); });
-    map.emplace(std::make_pair(tc1.get(), info));
 }
 
 void Sensors::tc1Callback()
@@ -429,7 +407,7 @@ void Sensors::tc1Callback()
     sdLogger.log(data);
 }
 
-void Sensors::vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::vesselPressureInit()
 {
     vesselPressure = std::make_unique<TrafagPressureSensor>(
         [this]()
@@ -442,10 +420,6 @@ void Sensors::vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::Trafag::VESSEL_MAX_PRESSURE,
         Config::Sensors::Trafag::MIN_CURRENT,
         Config::Sensors::Trafag::MAX_CURRENT);
-
-    SensorInfo info("VesselPressure", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { vesselPressureCallback(); });
-    map.emplace(std::make_pair(vesselPressure.get(), info));
 }
 
 void Sensors::vesselPressureCallback()
@@ -455,7 +429,7 @@ void Sensors::vesselPressureCallback()
     sdLogger.log(data);
 }
 
-void Sensors::fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::fillingPressureInit()
 {
     fillingPressure = std::make_unique<TrafagPressureSensor>(
         [this]()
@@ -468,11 +442,6 @@ void Sensors::fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::Trafag::FILLING_MAX_PRESSURE,
         Config::Sensors::Trafag::MIN_CURRENT,
         Config::Sensors::Trafag::MAX_CURRENT);
-
-    SensorInfo info("FillingPressure",
-                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { fillingPressureCallback(); });
-    map.emplace(std::make_pair(fillingPressure.get(), info));
 }
 
 void Sensors::fillingPressureCallback()
@@ -482,7 +451,7 @@ void Sensors::fillingPressureCallback()
     sdLogger.log(data);
 }
 
-void Sensors::topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::topTankPressureInit()
 {
     topTankPressure = std::make_unique<TrafagPressureSensor>(
         [this]()
@@ -495,11 +464,6 @@ void Sensors::topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::Trafag::TANK_TOP_MAX_PRESSURE,
         Config::Sensors::Trafag::MIN_CURRENT,
         Config::Sensors::Trafag::MAX_CURRENT);
-
-    SensorInfo info("TopTankPressure",
-                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { topTankPressureCallback(); });
-    map.emplace(std::make_pair(topTankPressure.get(), info));
 }
 
 void Sensors::topTankPressureCallback()
@@ -509,7 +473,7 @@ void Sensors::topTankPressureCallback()
     sdLogger.log(data);
 }
 
-void Sensors::bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::bottomTankPressureInit()
 {
     bottomTankPressure = std::make_unique<TrafagPressureSensor>(
         [this]()
@@ -522,11 +486,6 @@ void Sensors::bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::Trafag::TANK_BOTTOM_MAX_PRESSURE,
         Config::Sensors::Trafag::MIN_CURRENT,
         Config::Sensors::Trafag::MAX_CURRENT);
-
-    SensorInfo info("BottomTankPressure",
-                    Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { bottomTankPressureCallback(); });
-    map.emplace(std::make_pair(bottomTankPressure.get(), info));
 }
 
 void Sensors::bottomTankPressureCallback()
@@ -536,7 +495,7 @@ void Sensors::bottomTankPressureCallback()
     sdLogger.log(data);
 }
 
-void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::vesselWeightInit()
 {
     vesselWeight = std::make_unique<TwoPointAnalogLoadCell>(
         [this]()
@@ -549,10 +508,6 @@ void Sensors::vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::LoadCell::VESSEL_P0_MASS,
         Config::Sensors::LoadCell::VESSEL_P1_VOLTAGE,
         Config::Sensors::LoadCell::VESSEL_P1_MASS);
-
-    SensorInfo info("VesselWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { vesselWeightCallback(); });
-    map.emplace(std::make_pair(vesselWeight.get(), info));
 }
 
 void Sensors::vesselWeightCallback()
@@ -562,7 +517,7 @@ void Sensors::vesselWeightCallback()
     sdLogger.log(data);
 }
 
-void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map)
+void Sensors::tankWeightInit()
 {
     tankWeight = std::make_unique<TwoPointAnalogLoadCell>(
         [this]()
@@ -575,10 +530,6 @@ void Sensors::tankWeightInit(Boardcore::SensorManager::SensorMap_t &map)
         Config::Sensors::LoadCell::TANK_P0_MASS,
         Config::Sensors::LoadCell::TANK_P1_VOLTAGE,
         Config::Sensors::LoadCell::TANK_P1_MASS);
-
-    SensorInfo info("TankWeight", Config::Sensors::ADS131M08::SAMPLE_PERIOD,
-                    [this]() { tankWeightCallback(); });
-    map.emplace(std::make_pair(tankWeight.get(), info));
 }
 
 void Sensors::tankWeightCallback()
@@ -586,4 +537,79 @@ void Sensors::tankWeightCallback()
     LoadCellData sample = tankWeight->getLastSample();
     LCsData data{sample.loadTimestamp, 2, sample.load};
     sdLogger.log(data);
+}
+
+bool Sensors::sensorManagerInit()
+{
+    TaskScheduler &scheduler =
+        getModule<BoardScheduler>()->getSensorsScheduler();
+
+    SensorManager::SensorMap_t map;
+
+    if (internalAdc)
+    {
+        SensorInfo info("InternalAdc", Config::Sensors::InternalADC::PERIOD,
+                        [this]() { internalAdcCallback(); });
+        map.emplace(internalAdc.get(), info);
+    }
+
+    if (adc1)
+    {
+        SensorInfo info("ADS131M08_1", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { adc1Callback(); });
+        map.emplace(std::make_pair(adc1.get(), info));
+    }
+
+    if (tc1)
+    {
+        SensorInfo info("MAX31856_1", Config::Sensors::MAX31856::PERIOD,
+                        [this]() { tc1Callback(); });
+        map.emplace(std::make_pair(tc1.get(), info));
+    }
+
+    if (vesselPressure)
+    {
+        SensorInfo info("VesselPressure", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { vesselPressureCallback(); });
+        map.emplace(std::make_pair(vesselPressure.get(), info));
+    }
+
+    if (fillingPressure)
+    {
+        SensorInfo info("FillingPressure", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { fillingPressureCallback(); });
+        map.emplace(std::make_pair(fillingPressure.get(), info));
+    }
+
+    if (topTankPressure)
+    {
+        SensorInfo info("TopTankPressure", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { topTankPressureCallback(); });
+        map.emplace(std::make_pair(topTankPressure.get(), info));
+    }
+
+    if (bottomTankPressure)
+    {
+        SensorInfo info("BottomTankPressure",
+                        Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { bottomTankPressureCallback(); });
+        map.emplace(std::make_pair(bottomTankPressure.get(), info));
+    }
+
+    if (vesselWeight)
+    {
+        SensorInfo info("VesselWeight", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { vesselWeightCallback(); });
+        map.emplace(std::make_pair(vesselWeight.get(), info));
+    }
+
+    if (tankWeight)
+    {
+        SensorInfo info("TankWeight", Config::Sensors::ADS131M08::PERIOD,
+                        [this]() { tankWeightCallback(); });
+        map.emplace(std::make_pair(tankWeight.get(), info));
+    }
+
+    manager = std::make_unique<SensorManager>(map, &scheduler);
+    return manager->start();
 }
\ No newline at end of file
diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h
index 6cae9b512..48b583c7a 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -80,33 +80,35 @@ public:
     std::vector<Boardcore::SensorInfo> getSensorInfos();
 
 private:
-    void vesselPressureInit(Boardcore::SensorManager::SensorMap_t &map);
+    void vesselPressureInit();
     void vesselPressureCallback();
 
-    void fillingPressureInit(Boardcore::SensorManager::SensorMap_t &map);
+    void fillingPressureInit();
     void fillingPressureCallback();
 
-    void topTankPressureInit(Boardcore::SensorManager::SensorMap_t &map);
+    void topTankPressureInit();
     void topTankPressureCallback();
 
-    void bottomTankPressureInit(Boardcore::SensorManager::SensorMap_t &map);
+    void bottomTankPressureInit();
     void bottomTankPressureCallback();
 
-    void vesselWeightInit(Boardcore::SensorManager::SensorMap_t &map);
+    void vesselWeightInit();
     void vesselWeightCallback();
 
-    void tankWeightInit(Boardcore::SensorManager::SensorMap_t &map);
+    void tankWeightInit();
     void tankWeightCallback();
 
-    void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map);
+    void internalAdcInit();
     void internalAdcCallback();
 
-    void adc1Init(Boardcore::SensorManager::SensorMap_t &map);
+    void adc1Init();
     void adc1Callback();
 
-    void tc1Init(Boardcore::SensorManager::SensorMap_t &map);
+    void tc1Init();
     void tc1Callback();
 
+    bool sensorManagerInit();
+
     Boardcore::Logger &sdLogger   = Boardcore::Logger::getInstance();
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
 
-- 
GitLab