diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1b31cb98642320cf129d5f48817efa4a2bd20cd5..423224651c7ec72141228b7ad0bdfad1bc2917c3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,17 +36,7 @@ project(OnBoardSoftware)
 
 add_executable(main-entry src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
 target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(main-entry stm32f767zi_death_stack_v4)
-
-add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
-target_include_directories(main-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-roccaraso PRIVATE ROCCARASO)
-sbs_target(main-entry-roccaraso stm32f767zi_death_stack_v4)
-
-add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
-target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-euroc PRIVATE EUROC)
-sbs_target(main-entry-euroc stm32f767zi_death_stack_v4)
+sbs_target(main-entry stm32f767zi_bikeshed)
 
 add_executable(payload-entry-roccaraso src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER})
 target_include_directories(payload-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index dd49c5b21842e30d343d79cc3862abde119b812a..2d154e5a53395872276d03a50330545881e1850d 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -25,22 +25,22 @@ set(OBSW_INCLUDE_DIRS
 )
 
 set(MAIN_COMPUTER
-    src/boards/Main/BoardScheduler.cpp
+    # src/boards/Main/BoardScheduler.cpp
     src/boards/Main/Sensors/Sensors.cpp
-    src/boards/Main/StateMachines/NASController/NASController.cpp
+    # src/boards/Main/StateMachines/NASController/NASController.cpp
     src/boards/Main/Radio/Radio.cpp
-    src/boards/Main/TMRepository/TMRepository.cpp
-    src/boards/Main/CanHandler/CanHandler.cpp
-    src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
-    src/boards/Main/Actuators/Actuators.cpp
-    src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
-    src/boards/Main/StateMachines/ADAController/ADAController.cpp
-    src/boards/Main/PinHandler/PinHandler.cpp
-    src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
-    src/boards/Main/StateMachines/ABKController/ABKController.cpp
-    src/boards/Main/StateMachines/MEAController/MEAController.cpp
-    src/boards/Main/StateMachines/Deployment/Deployment.cpp
-    src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
+    # src/boards/Main/TMRepository/TMRepository.cpp
+    # src/boards/Main/CanHandler/CanHandler.cpp
+    # src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+    # src/boards/Main/Actuators/Actuators.cpp
+    # src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
+    # src/boards/Main/StateMachines/ADAController/ADAController.cpp
+    # src/boards/Main/PinHandler/PinHandler.cpp
+    # src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
+    # src/boards/Main/StateMachines/ABKController/ABKController.cpp
+    # src/boards/Main/StateMachines/MEAController/MEAController.cpp
+    # src/boards/Main/StateMachines/Deployment/Deployment.cpp
+    # src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
 )
 
 set(MOTOR_SOURCES
diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h
index 13552ec6be401c1d3be9ebd4456e4da4e94d3b9a..b48775aafa8c60c4259b53edb46199daf1eba25d 100644
--- a/src/boards/Main/Buses.h
+++ b/src/boards/Main/Buses.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -27,29 +27,39 @@
 #include <drivers/usart/USART.h>
 #include <interfaces-impl/hwmapping.h>
 
+#include <unordered_map>
 #include <utils/ModuleManager/ModuleManager.hpp>
+
 namespace Main
 {
+
 class Buses : public Boardcore::Module
 {
 public:
-    Boardcore::SPIBus spi1;
-    Boardcore::SPIBus spi3;
-    Boardcore::SPIBus spi4;
-    Boardcore::SPIBus spi6;
-
-    Boardcore::I2C i2c1;
-
-    Boardcore::USART usart1;
-    Boardcore::USART usart2;
-    Boardcore::USART uart4;
-
-    Buses()
-        : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6),
-          i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(),
-               miosix::interfaces::i2c1::sda::getPin()),
-          usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200)
-    {
-    }
+    Buses() {}
+
+    Boardcore::SPIBus &getH3LIS331DL() { return spi1; }
+    Boardcore::SPIBus &getLPS22DF() { return spi1; }
+    Boardcore::SPIBus &getLIS2MDL() { return spi3; }
+    Boardcore::SPIBus &getLSM6DSRX() { return spi3; }
+    Boardcore::SPIBus &getUBXGps() { return spi3; }
+    Boardcore::SPIBus &getADS131M08() { return spi4; }
+    Boardcore::SPIBus &getRadio() { return spi6; }
+
+    Boardcore::USART &getHILUart() { return usart4; }
+
+    Boardcore::I2C &getLPS28DFW() { return i2c1; }
+
+private:
+    Boardcore::SPIBus spi1{SPI1};
+    Boardcore::SPIBus spi3{SPI3};
+    Boardcore::SPIBus spi4{SPI4};
+    Boardcore::SPIBus spi6{SPI6};
+
+    Boardcore::USART usart4{UART4, 256000, 1024};
+
+    Boardcore::I2C i2c1{I2C1, miosix::interfaces::i2c1::scl::getPin(),
+                        miosix::interfaces::i2c1::sda::getPin()};
 };
+
 }  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h
index 5726888cfa7d5001c477f4bdacca5009748092ef..f63b800e9df95a70d9ea78914578d7efc334ca8c 100644
--- a/src/boards/Main/Configs/RadioConfig.h
+++ b/src/boards/Main/Configs/RadioConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,28 +19,31 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #pragma once
 
 #include <common/MavlinkGemini.h>
 
 namespace Main
 {
-namespace RadioConfig
+
+namespace Config
 {
-// Mavlink driver template parameters
-constexpr uint32_t RADIO_PKT_LENGTH     = 255;
-constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20;
-constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
 
-constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250;
+namespace Radio
+{
+
+static constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20;
+static constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+
+static constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
+static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
 
-constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50;
-constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10;
+static constexpr uint8_t MAV_SYSTEM_ID    = 171;
+static constexpr uint8_t MAV_COMPONENT_ID = 96;
 
-constexpr uint8_t MAV_SYSTEM_ID = 171;
-constexpr uint8_t MAV_COMP_ID   = 96;
+}
 
-constexpr uint32_t MAVLINK_QUEUE_SIZE = 10;
+}
 
-}  // namespace RadioConfig
-}  // namespace Main
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
index 0873209db35bdcdb5aa8b11ddad26dcceb0ba4c4..31cd704a45409433613835ea068b1078828293aa 100644
--- a/src/boards/Main/Configs/SensorsConfig.h
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,6 +19,7 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #pragma once
 
 #include <sensors/ADS131M08/ADS131M08.h>
@@ -27,95 +28,87 @@
 #include <sensors/LPS22DF/LPS22DF.h>
 #include <sensors/LPS28DFW/LPS28DFW.h>
 #include <sensors/LSM6DSRX/LSM6DSRX.h>
-#include <sensors/UBXGPS/UBXGPSSpi.h>
 
 namespace Main
 {
-namespace SensorsConfig
+
+namespace Config
+{
+
+namespace Sensors
+{
+
+namespace LPS22DF
 {
-constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4;
-constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100;
+static constexpr Boardcore::LPS22DF::AVG AVG = Boardcore::LPS22DF::AVG_4;
+static constexpr Boardcore::LPS22DF::ODR ODR = Boardcore::LPS22DF::ODR_100;
+static constexpr uint32_t PERIOD             = 20;  // [ms] 50Hz
+}  // namespace LPS22DF
 
-constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4;
-constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100;
-constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR =
+namespace LPS28DFW
+{
+static constexpr Boardcore::LPS28DFW::FullScaleRange FS =
     Boardcore::LPS28DFW::FS_1260;
+static constexpr Boardcore::LPS28DFW::AVG AVG = Boardcore::LPS28DFW::AVG_4;
+static constexpr Boardcore::LPS28DFW::ODR ODR = Boardcore::LPS28DFW::ODR_100;
+static constexpr uint32_t PERIOD              = 20;  // [ms] 50Hz
+}  // namespace LPS28DFW
 
-constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR =
+namespace H3LIS331DL
+{
+static constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR =
     Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
-constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU =
-    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
-constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR =
+static constexpr Boardcore::H3LIS331DLDefs::FullScaleRange FS =
     Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+static constexpr uint32_t PERIOD = 10;  // [ms] 100Hz
+}  // namespace H3LIS331DL
 
-constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE =
-    Boardcore::LIS2MDL::MD_CONTINUOUS;
-constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ;
-constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5;
-
-constexpr uint8_t UBXGPS_SAMPLE_RATE = 5;
+namespace LIS2MDL
+{
+static constexpr Boardcore::LIS2MDL::ODR ODR = Boardcore::LIS2MDL::ODR_100_HZ;
+static constexpr unsigned int TEMP_DIVIDER   = 5;
+static constexpr uint32_t PERIOD             = 10;  // [ms] 100Hz
+}  // namespace LIS2MDL
 
-constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU =
-    Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+namespace UBXGPS
+{
+static constexpr uint32_t PERIOD = 200;  // [ms] 5Hz
+}
 
-constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS =
+namespace LSM6DSRX
+{
+static constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE ACC_FS =
     Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16;
-constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR =
+static constexpr Boardcore::LSM6DSRXConfig::ACC_ODR ACC_ODR =
     Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416;
-constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE =
-    Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE ACC_OP_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE;
 
-constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS =
+static constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE GYR_FS =
     Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000;
-constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR =
+static constexpr Boardcore::LSM6DSRXConfig::GYR_ODR GYR_ODR =
     Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416;
+static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE GYR_OP_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE;
+
+static constexpr uint32_t PERIOD = 20;  // [ms] 50Hz
+}  // namespace LSM6DSRX
+
+namespace ADS131M08
+{
+static constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR =
+    Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+static constexpr bool GLOBAL_CHOP_MODE_EN = true;
+static constexpr uint32_t PERIOD          = 10;  // [ms] 100Hz
+}  // namespace ADS131M08
+
+namespace InternalADC
+{
+static constexpr uint32_t PERIOD = 100;  // [ms] 10Hz
+}
+
+}  // namespace Sensors
+
+}  // namespace Config
 
-constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE =
-    Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
-constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION
-    LSM6DSRX_FIFO_TIMESTAMP_DECIMATION =
-        Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
-constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR
-    LSM6DSRX_FIFO_TEMPERATURE_BDR =
-        Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
-
-constexpr Boardcore::ADS131M08Defs::OversamplingRatio
-    ADS131M08_OVERSAMPLING_RATIO =
-        Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
-constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true;
-
-constexpr uint32_t LPS22DF_PERIOD         = 20;  // [ms] 50Hz
-constexpr uint32_t LPS28DFW_PERIOD        = 20;  // [ms] 50Hz
-constexpr uint32_t H3LIS331DL_PERIOD      = 10;  // [ms] 100Hz
-constexpr uint32_t LIS2MDL_PERIOD         = 10;  // [ms] 100Hz
-constexpr uint32_t UBXGPS_PERIOD          = 1000 / UBXGPS_SAMPLE_RATE;  // [ms]
-constexpr uint32_t LSM6DSRX_PERIOD        = 20;  // [ms] 50Hz
-constexpr uint32_t ADS131M08_PERIOD       = 10;  // [ms] 100Hz
-constexpr uint32_t IMU_PERIOD             = 20;  // [ms] Fake processed IMU 50Hz
-constexpr uint32_t MAG_CALIBRATION_PERIOD = 20;  // [ms] 50Hz
-
-// ADC sensors configs
-constexpr float ADC_VOLTAGE_RANGE = 1.2f;  // [V] Voltage reading range
-constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_3;
-constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
-constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
-
-// ADC Voltage divider
-constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR =
-    (20.f / 2.4f) +
-    1;  // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider
-constexpr float CURRENT_CONVERSION_FACTOR =
-    (20.f / 4.f) / (12.f / (12.f + 33.f));
-constexpr float CURRENT_OFFSET = 0.133333333f;  // V in ADC
-
-// Calibration samples
-constexpr unsigned int CALIBRATION_SAMPLES = 20;
-constexpr unsigned int CALIBRATION_PERIOD  = 100;
-
-constexpr unsigned int NUMBER_OF_SENSORS = 8;
-
-}  // namespace SensorsConfig
 }  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index f3c6b40f4bd6076d4cd71c8670b49f5cf9824282..4a6d45fc413a6dfdc8f06e4ddf9d395a3c3d819f 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,479 +19,110 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-#include <Main/Actuators/Actuators.h>
-#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+
+#include "Radio.h"
+
 #include <Main/Buses.h>
-#include <Main/PinHandler/PinHandler.h>
-#include <Main/Radio/Radio.h>
-#include <Main/Sensors/Sensors.h>
-#include <Main/StateMachines/ADAController/ADAController.h>
-#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Main/StateMachines/NASController/NASController.h>
-#include <Main/TMRepository/TMRepository.h>
-#include <common/Events.h>
-#include <common/Topics.h>
-#include <drivers/interrupt/external_interrupts.h>
-#include <events/EventBroker.h>
+#include <common/Radio.h>
 #include <radio/SX1278/SX1278Frontends.h>
 
-#include <Eigen/Core>
-
+using namespace Main;
 using namespace Boardcore;
+using namespace miosix;
 using namespace Common;
-using namespace Eigen;
 
-#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl
-#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
-#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl
+SX1278Fsk* gRadio{nullptr};
 
-void __attribute__((used)) SX1278_IRQ_DIO0()
+void handleDioIRQ()
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<Main::Radio>()->transceiver)
+    SX1278Fsk* instance = gRadio;
+    if (instance)
     {
-        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+        instance->handleDioIRQ();
     }
 }
 
-void __attribute__((used)) SX1278_IRQ_DIO1()
+void setIRQRadio(SX1278Fsk* radio)
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<Main::Radio>()->transceiver)
-    {
-        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
-    }
+    FastInterruptDisableLock dl;
+    gRadio = radio;
 }
 
-void __attribute__((used)) SX1278_IRQ_DIO3()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<Main::Radio>()->transceiver)
-    {
-        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
-    }
-}
-namespace Main
-{
+void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); }
 
-Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
+bool Radio::isStarted() { return started; }
 
 bool Radio::start()
 {
     ModuleManager& modules = ModuleManager::getInstance();
 
-    // Config the transceiver
-    SX1278Fsk::Config config;
-    config.freq_rf    = 419000000;
-    config.freq_dev   = 50000;
-    config.bitrate    = 48000;
-    config.rx_bw      = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
-    config.afc_bw     = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
-    config.ocp        = 120;
-    config.power      = 13;
-    config.shaping    = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0;
-    config.dc_free    = Boardcore::SX1278Fsk::Config::DcFree::WHITENING;
-    config.enable_crc = false;
-
+    // Setup the frontend
     std::unique_ptr<SX1278::ISX1278Frontend> frontend =
         std::make_unique<Skyward433Frontend>();
 
-    transceiver = new SX1278Fsk(
-        modules.get<Buses>()->spi6, miosix::radio::cs::getPin(),
-        miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(),
-        miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64,
-        std::move(frontend));
+    // Setup transceiver
+    radio = std::make_unique<SX1278Fsk>(
+        modules.get<Buses>()->getRadio(), radio::cs::getPin(),
+        radio::dio0::getPin(), radio::dio1::getPin(), radio::dio3::getPin(),
+        SPI::ClockDivider::DIV_64, std::move(frontend));
 
-    // Config the radio
-    SX1278Fsk::Error error = transceiver->init(config);
+    // Store the global radio instance
+    setIRQRadio(radio.get());
 
-    // Add periodic telemetry send task
-    uint8_t result1 =
-        scheduler->addTask([&]() { this->sendPeriodicMessage(); },
-                           RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
-                           TaskScheduler::Policy::RECOVER);
-    uint8_t result2 = scheduler->addTask(
-        [&]()
-        {
-            this->enqueueMsg(
-                modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0));
-        },
-        RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2,
-        TaskScheduler::Policy::RECOVER);
+    // Initialize radio
+    auto result = radio->init(MAIN_RADIO_CONFIG);
+    if (result != SX1278Fsk::Error::NONE)
+    {
+        LOG_ERR(logger, "Failed to initialize Main radio");
+        return false;
+    }
 
-    // Config mavDriver
-    mavDriver = new MavDriver(
-        transceiver,
-        [=](MavDriver*, const mavlink_message_t& msg)
-        { this->handleMavlinkMessage(msg); },
-        RadioConfig::RADIO_SLEEP_AFTER_SEND,
-        RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
+    // Initialize mavdriver
+    mavDriver = std::make_unique<MavDriver>(
+        radio.get(),
+        [this](MavDriver*, const mavlink_message_t& msg)
+        { handleMessage(msg); },
+        Config::Radio::MAV_SLEEP_AFTER_SEND,
+        Config::Radio::MAV_OUT_BUFFER_MAX_AGE);
 
-    // Check radio failure
-    if (error != SX1278Fsk::Error::NONE)
+    if (!mavDriver->start())
     {
+        LOG_ERR(logger, "Failed to initialize Main mav driver");
         return false;
     }
 
-    // Start the mavlink driver thread
-    return mavDriver->start() && result1 != 0 && result2 != 0;
+    started = true;
+    return true;
+}
+
+Boardcore::MavlinkStatus Radio::getMavStatus()
+{
+    return mavDriver->getStatus();
 }
 
 void Radio::sendAck(const mavlink_message_t& msg)
 {
     mavlink_message_t ackMsg;
-    mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
-                            RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid,
+    mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                            Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid,
                             msg.seq);
-    enqueueMsg(ackMsg);
+    mavDriver->enqueueMsg(ackMsg);
 }
 
 void Radio::sendNack(const mavlink_message_t& msg)
 {
     mavlink_message_t nackMsg;
-    mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
-                             RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
-                             msg.seq);
-    enqueueMsg(nackMsg);
+    mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                             Config::Radio::MAV_COMPONENT_ID, &nackMsg,
+                             msg.msgid, msg.seq);
+    mavDriver->enqueueMsg(nackMsg);
 }
 
-void Radio::logStatus() {}
-
-bool Radio::isStarted()
+void Radio::handleMessage(const mavlink_message_t& msg)
 {
-    return mavDriver->isStarted() && scheduler->isRunning();
-}
-
-void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    switch (msg.msgid)
-    {
-        case MAVLINK_MSG_ID_PING_TC:
-        {
-            // Do nothing, just add the ack to the queue
-            break;
-        }
-        case MAVLINK_MSG_ID_COMMAND_TC:
-        {
-            // Let the handle command reply to the message
-            return handleCommand(msg);
-        }
-        case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
-        {
-            SystemTMList tmId = static_cast<SystemTMList>(
-                mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
-
-            if (tmId == MAV_PIN_OBS_ID)
-            {
-                for (uint8_t i = 0; i < PinHandler::PIN_END; i++)
-                {
-                    mavlink_message_t msg;
-                    mavlink_pin_tm_t tm;
-
-                    PinData data = modules.get<PinHandler>()->getPinData(
-                        static_cast<PinHandler::PinList>(i));
-
-                    tm.changes_counter       = data.changesCount;
-                    tm.current_state         = tm.current_state;
-                    tm.last_change_timestamp = tm.last_change_timestamp;
-                    tm.pin_id                = tm.pin_id;
-                    tm.timestamp             = TimestampTimer::getTimestamp();
-
-                    mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                              RadioConfig::MAV_COMP_ID, &msg,
-                                              &tm);
-                    enqueueMsg(msg);
-                }
-            }
-            else if (tmId == MAV_SENSORS_STATE_ID)
-            {
-                auto sensorsState = modules.get<Sensors>()->getSensorInfo();
-
-                for (uint8_t i = 0; i < sensorsState.size(); i++)
-                {
-                    mavlink_message_t msg;
-                    mavlink_sensor_state_tm_t tm;
-
-                    strcpy(tm.sensor_name, sensorsState.at(i).id.c_str());
-                    tm.state = sensorsState.at(i).isInitialized;
-
-                    mavlink_msg_sensor_state_tm_encode(
-                        RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
-                        &msg, &tm);
-                    enqueueMsg(msg);
-                }
-            }
-            else
-            {
-                // Add to the queue the respose
-                mavlink_message_t response =
-                    modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid,
-                                                              msg.seq);
-                // Add the response to the queue
-                enqueueMsg(response);
-
-                // Check if the TM repo answered with a NACK. If so the function
-                // must return to avoid sending a default ack
-                if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-                {
-                    return;
-                }
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
-        {
-            SensorsTMList sensorId = static_cast<SensorsTMList>(
-                mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
-            mavlink_message_t response =
-                modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid,
-                                                           msg.seq);
-            enqueueMsg(response);
-
-            // If the response is a nack the method returns
-            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-            {
-                return;
-            }
-            break;
-        }
-        case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
-        {
-            ServosList servo = static_cast<ServosList>(
-                mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
-            mavlink_message_t response =
-                modules.get<TMRepository>()->packServoTm(servo, msg.msgid,
-                                                         msg.seq);
-            enqueueMsg(response);
-
-            // If the response is a nack the method returns
-            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-            {
-                return;
-            }
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
-            float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
-
-            // Send nack if the FMM is not in test mode
-            if (!modules.get<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode))
-            {
-                return sendNack(msg);
-            }
-
-            // If the state is test mode, the servo is set to the correct angle
-            modules.get<Actuators>()->setServoPosition(servoId, position);
-            break;
-        }
-        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
-
-            // Send nack if the FMM is not in test mode
-            if (!modules.get<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode))
-            {
-                return sendNack(msg);
-            }
-
-            // If the state is test mode, the wiggle is done
-            modules.get<Actuators>()->wiggleServo(servoId);
-
-            break;
-        }
-        case MAVLINK_MSG_ID_RESET_SERVO_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_reset_servo_tc_get_servo_id(&msg));
+    LOG_INFO(logger, "Radio received data");
 
-            // Send nack if the FMM is not in test mode
-            if (!modules.get<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode))
-            {
-                return sendNack(msg);
-            }
-
-            // Set the servo position to 0
-            modules.get<Actuators>()->setServoPosition(servoId, 0);
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
-        {
-            float altitude =
-                mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
-
-            modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
-        {
-            float altitude =
-                mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
-
-            modules.get<ADAController>()->setReferenceAltitude(altitude);
-            modules.get<NASController>()->setReferenceAltitude(altitude);
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
-        {
-            float temperature =
-                mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
-
-            modules.get<ADAController>()->setReferenceTemperature(temperature);
-            modules.get<NASController>()->setReferenceTemperature(temperature);
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_COORDINATES_TC:
-        {
-            float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
-            float longitude =
-                mavlink_msg_set_coordinates_tc_get_longitude(&msg);
-
-            Vector2f coordinates{latitude, longitude};
-
-            modules.get<NASController>()->setCoordinates(coordinates);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_ORIENTATION_TC:
-        {
-            float yaw   = mavlink_msg_set_orientation_tc_get_yaw(&msg);
-            float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg);
-            float roll  = mavlink_msg_set_orientation_tc_get_roll(&msg);
-
-            modules.get<NASController>()->setOrientation(yaw, pitch, roll);
-            break;
-        }
-        default:
-        {
-            LOG_DEBUG(logger, "Received message is not of a known type");
-            return sendNack(msg);
-        }
-    }
-
-    // At the end send the ack message
-    sendAck(msg);
-}
-
-void Radio::handleCommand(const mavlink_message_t& msg)
-{
-    MavCommandList commandId = static_cast<MavCommandList>(
-        mavlink_msg_command_tc_get_command_id(&msg));
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Create the map between the commands and the corresponding events
-    static const std::map<MavCommandList, Events> commandToEvent{
-        {MAV_CMD_ARM, TMTC_ARM},
-        {MAV_CMD_DISARM, TMTC_DISARM},
-        {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
-        {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
-        {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
-        {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
-        {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
-        {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
-        {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
-        {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
-        {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE},
-        {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE},
-        {MAV_CMD_START_RECORDING, TMTC_START_RECORDING},
-        {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING},
-    };
-
-    switch (commandId)
-    {
-        case MAV_CMD_SAVE_CALIBRATION:
-        {
-            // Save the sensor calibration and adopt it
-            bool result = modules.get<Sensors>()->writeMagCalibration();
-
-            if (!result)
-            {
-                return sendNack(msg);
-            }
-
-            break;
-        }
-        case MAV_CMD_START_LOGGING:
-        {
-            bool result = Logger::getInstance().start();
-
-            // In case the logger is not started send to GS the result
-            if (!result)
-            {
-                return sendNack(msg);
-            }
-            break;
-        }
-        case MAV_CMD_STOP_LOGGING:
-        {
-            Logger::getInstance().stop();
-            break;
-        }
-        default:
-        {
-            // If the command is not a particular one, look for it inside the
-            // map
-            auto it = commandToEvent.find(commandId);
-
-            if (it != commandToEvent.end())
-            {
-                EventBroker::getInstance().post(it->second, TOPIC_TMTC);
-            }
-            else
-            {
-                return sendNack(msg);
-            }
-        }
-    }
-    // Acknowledge the message
     sendAck(msg);
-}
-
-void Radio::sendPeriodicMessage()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Send all the queue messages
-    {
-        Lock<FastMutex> lock(queueMutex);
-
-        for (uint8_t i = 0; i < messageQueueIndex; i++)
-        {
-            mavDriver->enqueueMsg(messageQueue[i]);
-        }
-
-        // Reset the index
-        messageQueueIndex = 0;
-    }
-
-    mavDriver->enqueueMsg(
-        modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0));
-    mavDriver->enqueueMsg(
-        modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0));
-}
-
-void Radio::enqueueMsg(const mavlink_message_t& msg)
-{
-    {
-        Lock<FastMutex> lock(queueMutex);
-
-        // Insert the message inside the queue only if there is enough space
-        if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE)
-        {
-            messageQueue[messageQueueIndex] = msg;
-            messageQueueIndex++;
-        }
-    }
-}
-}  // namespace Main
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
index 9d7a5e80eb1332e9a8e3c6d091d22ed34f9c1791..457e84a7c7c74ecfc4d03f48c3c2610833218fd9 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,84 +19,45 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #pragma once
 
 #include <Main/Configs/RadioConfig.h>
-#include <common/MavlinkGemini.h>
+#include <common/Mavlink.h>
 #include <radio/MavlinkDriver/MavlinkDriver.h>
 #include <radio/SX1278/SX1278Fsk.h>
-#include <scheduler/TaskScheduler.h>
 
 #include <utils/ModuleManager/ModuleManager.hpp>
 
 namespace Main
 {
+
 using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
-                                           RadioConfig::RADIO_OUT_QUEUE_SIZE,
-                                           RadioConfig::RADIO_MAV_MSG_LENGTH>;
+                                           Config::Radio::MAV_OUT_QUEUE_SIZE,
+                                           Config::Radio::MAV_MAX_LENGTH>;
 
 class Radio : public Boardcore::Module
 {
 public:
-    Radio(Boardcore::TaskScheduler* sched);
-
-    /**
-     * @brief Starts the MavlinkDriver
-     */
-    [[nodiscard]] bool start();
-
-    /**
-     * @brief Sends via radio an acknowledge message about the parameter passed
-     * message
-     */
-    void sendAck(const mavlink_message_t& msg);
-
-    /**
-     * @brief Sends via radio an non-acknowledge message about the parameter
-     * passed message
-     */
-    void sendNack(const mavlink_message_t& msg);
-
-    /**
-     * @brief Saves the MavlinkDriver and transceiver status
-     */
-    void logStatus();
+    Radio() {}
 
-    /**
-     * @brief Returns if the radio module is correctly started
-     */
     bool isStarted();
 
-    Boardcore::SX1278Fsk* transceiver = nullptr;
-    MavDriver* mavDriver              = nullptr;
-
-private:
-    /**
-     * @brief Called by the MavlinkDriver when a message is received
-     */
-    void handleMavlinkMessage(const mavlink_message_t& msg);
-
-    /**
-     * @brief Called by the handleMavlinkMessage to handle a command message
-     */
-    void handleCommand(const mavlink_message_t& msg);
+    [[nodiscard]] bool start();
 
-    /**
-     * @brief Sends the periodic telemetry
-     */
-    void sendPeriodicMessage();
+    Boardcore::MavlinkStatus getMavStatus();
 
-    /**
-     * @brief Inserts the mavlink message into the queue
-     */
-    void enqueueMsg(const mavlink_message_t& msg);
+private:
+    void sendAck(const mavlink_message_t& msg);
+    void sendNack(const mavlink_message_t& msg);
 
-    // Messages queue
-    mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE];
-    uint32_t messageQueueIndex = 0;
-    miosix::FastMutex queueMutex;
+    void handleMessage(const mavlink_message_t& msg);
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
-    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    std::atomic<bool> started{false};
+    std::unique_ptr<Boardcore::SX1278Fsk> radio;
+    std::unique_ptr<MavDriver> mavDriver;
 };
+
 }  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index 2333ee4e1a3748105e91febe1245ce29d9425bfd..c1d01a993f2ad91afacfe49f7c8fe47e9b4688bb 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -26,710 +26,240 @@
 #include <Main/Configs/SensorsConfig.h>
 #include <interfaces-impl/hwmapping.h>
 
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Main;
 using namespace Boardcore;
-using namespace std;
-using namespace Main::SensorsConfig;
-namespace Main
-{
-LPS22DFData Sensors::getLPS22DFLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
-}
-LPS28DFWData Sensors::getLPS28DFW_1LastSample()
-{
-    miosix::PauseKernelLock lock;
-    return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{};
-}
-LPS28DFWData Sensors::getLPS28DFW_2LastSample()
-{
-    miosix::PauseKernelLock lock;
-    return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{};
-}
-H3LIS331DLData Sensors::getH3LIS331DLLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
-                                 : H3LIS331DLData{};
-}
-LIS2MDLData Sensors::getLIS2MDLLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
-}
-UBXGPSData Sensors::getGPSLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{};
-}
-LSM6DSRXData Sensors::getLSM6DSRXLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
-}
-ADS131M08Data Sensors::getADS131M08LastSample()
-{
-    miosix::PauseKernelLock lock;
-    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
-}
+using namespace miosix;
 
-PitotData Sensors::getPitotLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return canPitot;
-}
-PressureData Sensors::getCCPressureLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return canCCPressure;
-}
-PressureData Sensors::getBottomTankPressureLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return canBottomTankPressure;
-}
-PressureData Sensors::getTopTankPressureLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return canTopTankPressure;
-}
-TemperatureData Sensors::getTankTemperatureLastSample()
-{
-    miosix::PauseKernelLock lock;
-    return canTankTemperature;
-}
-BatteryVoltageSensorData Sensors::getMotorBatteryVoltage()
-{
-    miosix::PauseKernelLock lock;
-    return canMotorBatteryVoltage;
-}
-CurrentData Sensors::getMotorCurrent()
-{
-    miosix::PauseKernelLock lock;
-    return canMotorCurrent;
-}
+bool Sensors::isStarted() { return started; }
 
-// Processed Getters
-BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
+bool Sensors::start()
 {
-    // Do not need to pause the kernel, the last sample getter is already
-    // protected
-    ADS131M08Data sample = getADS131M08LastSample();
-    BatteryVoltageSensorData data;
-
-    // Populate the data
-    data.voltageTimestamp = sample.timestamp;
-    data.channelId        = 1;
-    data.voltage          = sample.voltage[1];
-    data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
-    return data;
-}
+    SensorManager::SensorMap_t map;
+    lps22dfInit(map);
+    lps28dfwInit(map);
+    h3lis331dlInit(map);
+    lis2mdlInit(map);
+    ubxgpsInit(map);
+    lsm6dsrxInit(map);
+    ads131m08Init(map);
+    internalAdcInit(map);
+
+    manager = std::make_unique<SensorManager>(map, &scheduler);
+    if (!manager->start())
+    {
+        return false;
+    }
 
-BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample()
-{
-    // Do not need to pause the kernel, the last sample getter is already
-    // protected
-    ADS131M08Data sample = getADS131M08LastSample();
-    BatteryVoltageSensorData data;
-
-    // Populate the data
-    data.voltageTimestamp = sample.timestamp;
-    data.channelId        = 7;
-    data.voltage          = sample.voltage[7];
-    data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
-    return data;
+    started = true;
+    return true;
 }
 
-CurrentData Sensors::getCurrentLastSample()
+std::vector<Boardcore::SensorInfo> Sensors::getSensorInfo()
 {
-    // Do not need to pause the kernel, the last sample getter is already
-    // protected
-    ADS131M08Data sample = getADS131M08LastSample();
-    CurrentData data;
-
-    // Populate the data
-    data.currentTimestamp = sample.timestamp;
-    data.current =
-        (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR;
-    return data;
+    return {manager->getSensorInfo(lps22df.get()),
+            manager->getSensorInfo(lps28dfw.get()),
+            manager->getSensorInfo(h3lis331dl.get()),
+            manager->getSensorInfo(lis2mdl.get()),
+            manager->getSensorInfo(ubxgps.get()),
+            manager->getSensorInfo(lsm6dsrx.get()),
+            manager->getSensorInfo(ads131m08.get()),
+            manager->getSensorInfo(internalAdc.get())};
 }
 
-MPXH6400AData Sensors::getDeploymentPressureLastSample()
+void Sensors::lps22dfInit(SensorManager::SensorMap_t &map)
 {
-    miosix::PauseKernelLock lock;
-    return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{};
-}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-HSCMRNN015PAData Sensors::getStaticPressure1LastSample()
-{
-    miosix::PauseKernelLock lock;
-    return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample()
-                                     : HSCMRNN015PAData{};
-}
+    SPIBusConfig spiConfig = LPS22DF::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-HSCMRNN015PAData Sensors::getStaticPressure2LastSample()
-{
-    miosix::PauseKernelLock lock;
-    return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample()
-                                     : HSCMRNN015PAData{};
-}
+    LPS22DF::Config config;
+    config.avg = Config::Sensors::LPS22DF::AVG;
+    config.odr = Config::Sensors::LPS22DF::ODR;
 
-RotatedIMUData Sensors::getIMULastSample()
-{
-    miosix::PauseKernelLock lock;
-    return imu != nullptr ? imu->getLastSample() : RotatedIMUData{};
+    lps22df = std::make_unique<LPS22DF>(modules.get<Buses>()->getLPS22DF(),
+                                        sensors::LPS22DF::cs::getPin(),
+                                        spiConfig, config);
+
+    SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::PERIOD,
+                    [this]() { lps22dfCallback(); }};
+    map.emplace(lps22df.get(), info);
 }
 
-MagnetometerData Sensors::getCalibratedMagnetometerLastSample()
+void Sensors::lps22dfCallback()
 {
-    // Do not need to pause the kernel, the last sample getter is already
-    // protected
-    MagnetometerData lastSample = getLIS2MDLLastSample();
-    MagnetometerData result;
-
-    // Correct the result and copy the timestamp
-    {
-        miosix::Lock<FastMutex> l(calibrationMutex);
-        result =
-            static_cast<MagnetometerData>(magCalibration.correct(lastSample));
-    }
+    auto sample = lps22df->getLastSample();
 
-    result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp;
-    return result;
+    LOG_INFO(logger, "LPS22DF {}", sample.pressure);
 }
 
-void Sensors::setPitot(PitotData data)
-{
-    miosix::PauseKernelLock lock;
-    canPitot           = data;
-    canPitot.timestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setCCPressure(PressureData data)
+void Sensors::lps28dfwInit(SensorManager::SensorMap_t &map)
 {
-    miosix::PauseKernelLock lock;
-    canCCPressure                   = data;
-    canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setBottomTankPressure(PressureData data)
-{
-    miosix::PauseKernelLock lock;
-    canBottomTankPressure                   = data;
-    canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setTopTankPressure(PressureData data)
-{
-    miosix::PauseKernelLock lock;
-    canTopTankPressure                   = data;
-    canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setTankTemperature(TemperatureData data)
-{
-    miosix::PauseKernelLock lock;
-    canTankTemperature                      = data;
-    canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data)
-{
-    miosix::PauseKernelLock lock;
-    canMotorBatteryVoltage.batVoltage       = data.batVoltage;
-    canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp();
-}
-void Sensors::setMotorCurrent(CurrentData data)
-{
-    miosix::PauseKernelLock lock;
-    canMotorCurrent.current          = data.current;
-    canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp();
-}
-
-Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-bool Sensors::start()
-{
-    // Read the magnetometer calibration from predefined file
-    magCalibration.fromFile("/sd/magCalibration.csv");
-
-    // Init all the sensors
-    lps22dfInit();
-    lps28dfw_1Init();
-    // lps28dfw_2Init();
-    h3lis331dlInit();
-    lis2mdlInit();
-    ubxgpsInit();
-    lsm6dsrxInit();
-    ads131m08Init();
-    deploymentPressureInit();
-    staticPressure1Init();
-    staticPressure2Init();
-    imuInit();
-
-    // Add the magnetometer calibration to the scheduler
-    size_t result = scheduler->addTask(
-        [&]()
-        {
-            // Gather the last sample data
-            MagnetometerData lastSample = getLIS2MDLLastSample();
-
-            // Feed the data to the calibrator inside a protected area.
-            // Contention is not high and the use of a mutex is suitable to
-            // avoid pausing the kernel for this calibration operation
-            {
-                miosix::Lock<FastMutex> l(calibrationMutex);
-                magCalibrator.feed(lastSample);
-            }
-        },
-        MAG_CALIBRATION_PERIOD);
-
-    // Create sensor manager with populated map and configured scheduler
-    manager = new SensorManager(sensorMap, scheduler);
-    return manager->start() && result != 0;
-}
+    LPS28DFW::SensorConfig config;
+    config.sa0  = true;
+    config.fsr  = Config::Sensors::LPS28DFW::FS;
+    config.avg  = Config::Sensors::LPS28DFW::AVG;
+    config.odr  = Config::Sensors::LPS28DFW::ODR;
+    config.drdy = false;
 
-void Sensors::stop() { manager->stop(); }
+    lps28dfw =
+        std::make_unique<LPS28DFW>(modules.get<Buses>()->getLPS28DFW(), config);
 
-bool Sensors::isStarted()
-{
-    return manager->areAllSensorsInitialized() && scheduler->isRunning();
+    SensorInfo info{"LPS28DFW", Config::Sensors::LPS28DFW::PERIOD,
+                    [this]() { lps28dfwCallback(); }};
+    map.emplace(lps28dfw.get(), info);
 }
 
-void Sensors::calibrate()
+void Sensors::lps28dfwCallback()
 {
-    // Create the stats to calibrate the barometers
-    Stats lps28dfw1Stats;
-    // Stats lps28dfw2Stats;
-    Stats staticPressure1Stats;
-    Stats staticPressure2Stats;
-    Stats deploymentPressureStats;
-
-    // Add N samples to the stats
-    for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++)
-    {
-        lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure);
-        // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure);
-        staticPressure1Stats.add(getStaticPressure1LastSample().pressure);
-        staticPressure2Stats.add(getStaticPressure2LastSample().pressure);
-        deploymentPressureStats.add(getDeploymentPressureLastSample().pressure);
-
-        // Delay for the expected period
-        miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD);
-    }
+    auto sample = lps28dfw->getLastSample();
 
-    // Compute the difference between the mean value from LPS28DFW
-    float reference = lps28dfw1Stats.getStats().mean;
-
-    hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean -
-                                 reference);
-    hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean -
-                                 reference);
-    mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean -
-                            reference);
-
-    // Log the offsets
-    SensorsCalibrationParameter cal{};
-    cal.timestamp         = TimestampTimer::getTimestamp();
-    cal.offsetStatic1     = staticPressure1Stats.getStats().mean - reference;
-    cal.offsetStatic2     = staticPressure2Stats.getStats().mean - reference;
-    cal.offsetDeployment  = deploymentPressureStats.getStats().mean - reference;
-    cal.referencePressure = reference;
-
-    Logger::getInstance().log(cal);
+    LOG_INFO(logger, "LPS28DFW {}", sample.pressure);
 }
 
-bool Sensors::writeMagCalibration()
+void Sensors::h3lis331dlInit(SensorManager::SensorMap_t &map)
 {
-    // Compute the calibration result in protected area
-    {
-        miosix::Lock<FastMutex> l(calibrationMutex);
-        SixParametersCorrector cal = magCalibrator.computeResult();
-
-        // Check result validity
-        if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) &&
-            !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) &&
-            !isnan(cal.getA()[1]) && !isnan(cal.getA()[2]))
-        {
-            magCalibration = cal;
-
-            // Save the calibration to the calibration file
-            return magCalibration.toFile("/sd/magCalibration.csv");
-        }
-        return false;
-    }
-}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
-Sensors::getSensorInfo()
-{
-    std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState;
-    for (size_t i = 0; i < sensorsInit.size(); i++)
-    {
-        // Check wether the lambda is existent
-        if (sensorsInit[i])
-        {
-            sensorState[i] = sensorsInit[i]();
-        }
-    }
-    return sensorState;
-}
-
-void Sensors::lps22dfInit()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Get the correct SPI configuration
-    SPIBusConfig config = LPS22DF::getDefaultSPIConfig();
-    config.clockDivider = SPI::ClockDivider::DIV_16;
-
-    // Configure the device
-    LPS22DF::Config sensorConfig;
-    sensorConfig.avg = LPS22DF_AVG;
-    sensorConfig.odr = LPS22DF_ODR;
-
-    // Create sensor instance with configured parameters
-    lps22df = new LPS22DF(modules.get<Buses>()->spi3,
-                          miosix::sensors::LPS22DF::cs::getPin(), config,
-                          sensorConfig);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("LPS22DF", LPS22DF_PERIOD,
-                    bind(&Sensors::lps22dfCallback, this));
-    sensorMap.emplace(make_pair(lps22df, info));
-
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(lps22df); };
-}
-void Sensors::lps28dfw_1Init()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
+    SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-    // Configure the sensor
-    LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG,
-                                  LPS28DFW_ODR, false};
+    h3lis331dl = std::make_unique<H3LIS331DL>(
+        modules.get<Buses>()->getH3LIS331DL(),
+        sensors::H3LIS331DL::cs::getPin(), Config::Sensors::H3LIS331DL::ODR,
+        H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE,
+        Config::Sensors::H3LIS331DL::FS);
 
-    // Create sensor instance with configured parameters
-    lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+    SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::PERIOD,
+                    [this]() { h3lis331dlCallback(); }};
+    map.emplace(h3lis331dl.get(), info);
+}
 
-    // Emplace the sensor inside the map
-    SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD,
-                    bind(&Sensors::lps28dfw_1Callback, this));
-    sensorMap.emplace(make_pair(lps28dfw_1, info));
+void Sensors::h3lis331dlCallback() {}
 
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(lps28dfw_1); };
-}
-void Sensors::lps28dfw_2Init()
+void Sensors::lis2mdlInit(SensorManager::SensorMap_t &map)
 {
-    ModuleManager& modules = ModuleManager::getInstance();
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    // Configure the sensor
-    LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG,
-                                  LPS28DFW_ODR, false};
+    SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
 
-    // Create sensor instance with configured parameters
-    lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+    LIS2MDL::Config config;
+    config.deviceMode         = LIS2MDL::MD_CONTINUOUS;
+    config.odr                = Config::Sensors::LIS2MDL::ODR;
+    config.temperatureDivider = Config::Sensors::LIS2MDL::TEMP_DIVIDER;
 
-    // Emplace the sensor inside the map
-    SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD,
-                    bind(&Sensors::lps28dfw_2Callback, this));
-    sensorMap.emplace(make_pair(lps28dfw_2, info));
+    lis2mdl = std::make_unique<LIS2MDL>(modules.get<Buses>()->getLIS2MDL(),
+                                        sensors::LIS2MDL::cs::getPin(),
+                                        spiConfig, config);
 
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(lps28dfw_2); };
+    SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::PERIOD,
+                    [this]() { lis2mdlCallback(); }};
+    map.emplace(lis2mdl.get(), info);
 }
-void Sensors::h3lis331dlInit()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
 
-    // Get the correct SPI configuration
-    SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig();
-    config.clockDivider = SPI::ClockDivider::DIV_16;
+void Sensors::lis2mdlCallback() {}
 
-    // Create sensor instance with configured parameters
-    h3lis331dl = new H3LIS331DL(
-        modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(),
-        config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR);
+void Sensors::ubxgpsInit(SensorManager::SensorMap_t &map)
+{
+    ModuleManager &modules = ModuleManager::getInstance();
+
+    SPIBusConfig spiConfig = UBXGPSSpi::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
 
-    // Emplace the sensor inside the map
-    SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD,
-                    bind(&Sensors::h3lis331dlCallback, this));
-    sensorMap.emplace(make_pair(h3lis331dl, info));
+    ubxgps = std::make_unique<UBXGPSSpi>(modules.get<Buses>()->getUBXGps(),
+                                         sensors::UBXGps::cs::getPin(),
+                                         spiConfig, 5);
 
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(h3lis331dl); };
+    SensorInfo info{"UBXGPS", Config::Sensors::UBXGPS::PERIOD,
+                    [this]() { ubxgpsCallback(); }};
+    map.emplace(ubxgps.get(), info);
 }
-void Sensors::lis2mdlInit()
+
+void Sensors::ubxgpsCallback()
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Get the correct SPI configuration
-    SPIBusConfig config = LIS2MDL::getDefaultSPIConfig();
-    config.clockDivider = SPI::ClockDivider::DIV_16;
-
-    // Configure the sensor
-    LIS2MDL::Config sensorConfig;
-    sensorConfig.deviceMode         = LIS2MDL_OPERATIVE_MODE;
-    sensorConfig.odr                = LIS2MDL_ODR;
-    sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER;
-
-    // Create sensor instance with configured parameters
-    lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3,
-                          miosix::sensors::LIS2MDL::cs::getPin(), config,
-                          sensorConfig);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("LIS2MDL", LIS2MDL_PERIOD,
-                    bind(&Sensors::lis2mdlCallback, this));
-    sensorMap.emplace(make_pair(lis2mdl, info));
-
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(lis2mdl); };
+    auto sample = ubxgps->getLastSample();
+
+    LOG_INFO(logger, "UBXGPS {} {} {} {}", sample.fix, sample.satellites,
+             sample.latitude, sample.longitude);
 }
 
-void Sensors::ubxgpsInit()
+void Sensors::lsm6dsrxInit(SensorManager::SensorMap_t &map)
 {
-    ModuleManager& modules = ModuleManager::getInstance();
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    // Get the correct SPI configuration
-    SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig();
-    config.clockDivider = SPI::ClockDivider::DIV_64;
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+    spiConfig.mode         = SPI::Mode::MODE_0;
 
-    // Create sensor instance with configured parameters
-    ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4,
-                           miosix::sensors::GPS::cs::getPin(), config, 5);
+    LSM6DSRXConfig config;
+    config.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
 
-    // Emplace the sensor inside the map
-    SensorInfo info("UBXGPS", UBXGPS_PERIOD,
-                    bind(&Sensors::ubxgpsCallback, this));
-    sensorMap.emplace(make_pair(ubxgps, info));
+    config.fsAcc     = Config::Sensors::LSM6DSRX::ACC_FS;
+    config.odrAcc    = Config::Sensors::LSM6DSRX::ACC_ODR;
+    config.opModeAcc = Config::Sensors::LSM6DSRX::ACC_OP_MODE;
 
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(ubxgps); };
-}
+    config.fsGyr     = Config::Sensors::LSM6DSRX::GYR_FS;
+    config.odrGyr    = Config::Sensors::LSM6DSRX::GYR_ODR;
+    config.opModeGyr = Config::Sensors::LSM6DSRX::GYR_OP_MODE;
 
-void Sensors::lsm6dsrxInit()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Configure the SPI
-    SPIBusConfig config;
-    config.clockDivider = SPI::ClockDivider::DIV_32;
-    config.mode         = SPI::Mode::MODE_0;
-
-    // Configure the sensor
-    LSM6DSRXConfig sensorConfig;
-    sensorConfig.bdu = LSM6DSRX_BDU;
-
-    // Accelerometer
-    sensorConfig.fsAcc     = LSM6DSRX_ACC_FS;
-    sensorConfig.odrAcc    = LSM6DSRX_ACC_ODR;
-    sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE;
-
-    // Gyroscope
-    sensorConfig.fsGyr     = LSM6DSRX_GYR_FS;
-    sensorConfig.odrGyr    = LSM6DSRX_GYR_ODR;
-    sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE;
-
-    // Fifo
-    sensorConfig.fifoMode                = LSM6DSRX_FIFO_MODE;
-    sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION;
-    sensorConfig.fifoTemperatureBdr      = LSM6DSRX_FIFO_TEMPERATURE_BDR;
-
-    // Create sensor instance with configured parameters
-    lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1,
-                            miosix::sensors::LSM6DSRX::cs::getPin(), config,
-                            sensorConfig);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD,
-                    bind(&Sensors::lsm6dsrxCallback, this));
-    sensorMap.emplace(make_pair(lsm6dsrx, info));
-
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(lsm6dsrx); };
-}
+    config.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+    config.fifoTimestampDecimation =
+        LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+    config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
 
-void Sensors::ads131m08Init()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Configure the SPI
-    SPIBusConfig config;
-    config.clockDivider = SPI::ClockDivider::DIV_32;
-
-    // Configure the device
-    ADS131M08::Config sensorConfig;
-    sensorConfig.oversamplingRatio     = ADS131M08_OVERSAMPLING_RATIO;
-    sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE;
-
-    // Create the sensor instance with configured parameters
-    ads131m08 = new ADS131M08(modules.get<Buses>()->spi4,
-                              miosix::sensors::ADS131::cs::getPin(), config,
-                              sensorConfig);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("ADS131M08", ADS131M08_PERIOD,
-                    bind(&Sensors::ads131m08Callback, this));
-    sensorMap.emplace(make_pair(ads131m08, info));
-
-    // Add the sensor info getter to the array
-    sensorsInit[sensorsId++] = [&]() -> SensorInfo
-    { return manager->getSensorInfo(ads131m08); };
-}
+    lsm6dsrx = std::make_unique<LSM6DSRX>(modules.get<Buses>()->getLSM6DSRX(),
+                                          sensors::LSM6DSRX::cs::getPin(),
+                                          spiConfig, config);
 
-void Sensors::deploymentPressureInit()
-{
-    // Create the lambda function to get the voltage
-    function<ADCData()> getVoltage = [&]()
-    {
-        // No need to synchronize, the sampling thread is the same
-        ADS131M08Data sample = ads131m08->getLastSample();
-        return sample.getVoltage(ADC_CH_DPL);
-    };
-
-    // Create the sensor instance with created function
-    mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("MPXH6400A", ADS131M08_PERIOD,
-                    bind(&Sensors::deploymentPressureCallback, this));
-    sensorMap.emplace(make_pair(mpxh6400a, info));
+    SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::PERIOD,
+                    [this]() { lsm6dsrxCallback(); }};
+    map.emplace(lsm6dsrx.get(), info);
 }
 
-void Sensors::staticPressure1Init()
-{
-    // Create the lambda function to get the voltage
-    function<ADCData()> getVoltage = [&]()
-    {
-        // No need to synchronize, the sampling thread is the same
-        ADS131M08Data sample = ads131m08->getLastSample();
-        return sample.getVoltage(ADC_CH_STATIC_1);
-    };
-
-    // Create the sensor instance with created function
-    hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD,
-                    bind(&Sensors::staticPressure1Callback, this));
-    sensorMap.emplace(make_pair(hscmrnn015pa_1, info));
-}
+void Sensors::lsm6dsrxCallback() {}
 
-void Sensors::staticPressure2Init()
+void Sensors::ads131m08Init(SensorManager::SensorMap_t &map)
 {
-    // Create the lambda function to get the voltage
-    function<ADCData()> getVoltage = [&]()
-    {
-        // No need to synchronize, the sampling thread is the same
-        ADS131M08Data sample = ads131m08->getLastSample();
-        return sample.getVoltage(ADC_CH_STATIC_2);
-    };
-
-    // Create the sensor instance with created function
-    hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
-
-    // Emplace the sensor inside the map
-    SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD,
-                    bind(&Sensors::staticPressure2Callback, this));
-    sensorMap.emplace(make_pair(hscmrnn015pa_2, info));
-}
+    ModuleManager &modules = ModuleManager::getInstance();
 
-void Sensors::imuInit()
-{
-    // Register the IMU as the fake sensor, passing as parameters the methods to
-    // retrieve real data. The sensor is not synchronized, but the sampling
-    // thread is always the same.
-    imu = new RotatedIMU(
-        bind(&LSM6DSRX::getLastSample, lsm6dsrx),
-        bind(&Sensors::getCalibratedMagnetometerLastSample, this),
-        bind(&LSM6DSRX::getLastSample, lsm6dsrx));
-
-    // Invert the Y axis on the magnetometer
-    Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}};
-    imu->addMagTransformation(m);
-    imu->addAccTransformation(imu->rotateAroundX(45));
-    imu->addGyroTransformation(imu->rotateAroundX(45));
-    imu->addMagTransformation(imu->rotateAroundX(45));
-
-    // Emplace the sensor inside the map
-    SensorInfo info("RotatedIMU", IMU_PERIOD,
-                    bind(&Sensors::imuCallback, this));
-    sensorMap.emplace(make_pair(imu, info));
-}
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
 
-void Sensors::lps22dfCallback()
-{
-    LPS22DFData lastSample = lps22df->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::lps28dfw_1Callback()
-{
-    LPS28DFW_1Data lastSample =
-        static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample());
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::lps28dfw_2Callback()
-{
-    LPS28DFW_2Data lastSample =
-        static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample());
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::h3lis331dlCallback()
-{
-    H3LIS331DLData lastSample = h3lis331dl->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::lis2mdlCallback()
-{
-    LIS2MDLData lastSample = lis2mdl->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::ubxgpsCallback()
-{
-    UBXGPSData lastSample = ubxgps->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::lsm6dsrxCallback()
-{
-    auto& fifo        = lsm6dsrx->getLastFifo();
-    uint16_t fifoSize = lsm6dsrx->getLastFifoSize();
+    ADS131M08::Config config;
+    config.oversamplingRatio = Config::Sensors::ADS131M08::OSR;
+    config.globalChopModeEnabled =
+        Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN;
 
-    // For every instance inside the fifo log the sample
-    for (uint16_t i = 0; i < fifoSize; i++)
-    {
-        Logger::getInstance().log(fifo.at(i));
-    }
-}
-void Sensors::ads131m08Callback()
-{
-    ADS131M08Data lastSample = ads131m08->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::deploymentPressureCallback()
-{
-    MPXH6400AData lastSample = mpxh6400a->getLastSample();
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::staticPressure1Callback()
-{
-    HSCMRNN015PA_1Data lastSample =
-        static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample());
-    Logger::getInstance().log(lastSample);
-}
-void Sensors::staticPressure2Callback()
-{
-    HSCMRNN015PA_2Data lastSample =
-        static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample());
-    Logger::getInstance().log(lastSample);
+    ads131m08 = std::make_unique<ADS131M08>(
+        modules.get<Buses>()->getADS131M08(), sensors::ADS131M08::cs::getPin(),
+        spiConfig, config);
+
+    SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::PERIOD,
+                    [this]() { ads131m08Callback(); }};
+    map.emplace(ads131m08.get(), info);
 }
-void Sensors::imuCallback()
+
+void Sensors::ads131m08Callback() {}
+
+void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
 {
-    RotatedIMUData lastSample = imu->getLastSample();
-    Logger::getInstance().log(lastSample);
+    ModuleManager &modules = ModuleManager::getInstance();
+
+    internalAdc = std::make_unique<InternalADC>(ADC2);
+    internalAdc->enableChannel(InternalADC::CH8);
+    internalAdc->enableChannel(InternalADC::CH9);
+    internalAdc->enableChannel(InternalADC::CH11);
+    internalAdc->enableTemperature();
+    internalAdc->enableVbat();
+
+    SensorInfo info{"InternalADC", Config::Sensors::InternalADC::PERIOD,
+                    [this]() { internalAdcCallback(); }};
+    map.emplace(internalAdc.get(), info);
 }
 
-}  // namespace Main
\ No newline at end of file
+void Sensors::internalAdcCallback() {}
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index 44ad52749f84cc0a750388f1581969611e76cdf2..ad6719ac9eacbd25901b7d4e7ad1a8608795ce5b 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,184 +19,82 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #pragma once
 
-#include <Main/Configs/SensorsConfig.h>
-#include <Main/Sensors/RotatedIMU/RotatedIMU.h>
-#include <Main/Sensors/SensorsData.h>
+#include <diagnostic/PrintLogger.h>
+#include <scheduler/TaskScheduler.h>
 #include <sensors/ADS131M08/ADS131M08.h>
 #include <sensors/H3LIS331DL/H3LIS331DL.h>
 #include <sensors/LIS2MDL/LIS2MDL.h>
 #include <sensors/LPS22DF/LPS22DF.h>
 #include <sensors/LPS28DFW/LPS28DFW.h>
 #include <sensors/LSM6DSRX/LSM6DSRX.h>
-#include <sensors/SensorData.h>
 #include <sensors/SensorManager.h>
 #include <sensors/UBXGPS/UBXGPSSpi.h>
-#include <sensors/analog/BatteryVoltageSensorData.h>
-#include <sensors/analog/Pitot/PitotData.h>
-#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h>
-#include <sensors/analog/pressure/nxp/MPXH6400A.h>
-#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
-#include <stdint.h>
+#include <drivers/adc/InternalADC.h>
 
+#include <memory>
 #include <utils/ModuleManager/ModuleManager.hpp>
+#include <vector>
 
 namespace Main
 {
+
 class Sensors : public Boardcore::Module
 {
 public:
-    explicit Sensors(Boardcore::TaskScheduler* sched);
-
-    [[nodiscard]] bool start();
+    explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
+    {
+    }
 
-    /**
-     * @brief Stops the sensor manager
-     * @warning Stops the passed scheduler
-     */
-    void stop();
-
-    /**
-     * @brief Returns if all the sensors are started successfully
-     */
     bool isStarted();
 
-    /**
-     * @brief Calibrates the sensors with an offset
-     */
-    void calibrate();
-
-    /**
-     * @brief Takes the result of the live magnetometer calibration and applies
-     * it to the current calibration + writes it in the csv file
-     */
-    bool writeMagCalibration();
-
-    // Sensor getters
-    Boardcore::LPS22DFData getLPS22DFLastSample();
-    Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
-    Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
-    Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
-    Boardcore::LIS2MDLData getLIS2MDLLastSample();
-    Boardcore::UBXGPSData getGPSLastSample();
-    Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
-    Boardcore::ADS131M08Data getADS131M08LastSample();
-
-    // Processed getters
-    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
-    Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
-    Boardcore::CurrentData getCurrentLastSample();
-    Boardcore::MPXH6400AData getDeploymentPressureLastSample();
-    Boardcore::HSCMRNN015PAData getStaticPressure1LastSample();
-    Boardcore::HSCMRNN015PAData getStaticPressure2LastSample();
-    RotatedIMUData getIMULastSample();
-    Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
-
-    // CAN fake sensors setters
-    void setPitot(Boardcore::PitotData data);
-    void setCCPressure(Boardcore::PressureData data);
-    void setBottomTankPressure(Boardcore::PressureData data);
-    void setTopTankPressure(Boardcore::PressureData data);
-    void setTankTemperature(Boardcore::TemperatureData data);
-    void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data);
-    void setMotorCurrent(Boardcore::CurrentData data);
-
-    // CAN fake sensors getters
-    Boardcore::PitotData getPitotLastSample();
-    Boardcore::PressureData getCCPressureLastSample();
-    Boardcore::PressureData getBottomTankPressureLastSample();
-    Boardcore::PressureData getTopTankPressureLastSample();
-    Boardcore::TemperatureData getTankTemperatureLastSample();
-    Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage();
-    Boardcore::CurrentData getMotorCurrent();
-
-    // Returns the sensors statuses
-    std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
-    getSensorInfo();
+    [[nodiscard]] bool start();
+
+    std::vector<Boardcore::SensorInfo> getSensorInfo();
 
 private:
-    // Init and callbacks methods
-    void lps22dfInit();
+    void lps22dfInit(Boardcore::SensorManager::SensorMap_t &map);
     void lps22dfCallback();
 
-    void lps28dfw_1Init();
-    void lps28dfw_1Callback();
+    void lps28dfwInit(Boardcore::SensorManager::SensorMap_t &map);
+    void lps28dfwCallback();
 
-    void lps28dfw_2Init();
-    void lps28dfw_2Callback();
-
-    void h3lis331dlInit();
+    void h3lis331dlInit(Boardcore::SensorManager::SensorMap_t &map);
     void h3lis331dlCallback();
 
-    void lis2mdlInit();
+    void lis2mdlInit(Boardcore::SensorManager::SensorMap_t &map);
     void lis2mdlCallback();
 
-    void ubxgpsInit();
+    void ubxgpsInit(Boardcore::SensorManager::SensorMap_t &map);
     void ubxgpsCallback();
 
-    void lsm6dsrxInit();
+    void lsm6dsrxInit(Boardcore::SensorManager::SensorMap_t &map);
     void lsm6dsrxCallback();
 
-    void ads131m08Init();
+    void ads131m08Init(Boardcore::SensorManager::SensorMap_t &map);
     void ads131m08Callback();
 
-    void deploymentPressureInit();
-    void deploymentPressureCallback();
-
-    void staticPressure1Init();
-    void staticPressure1Callback();
-
-    void staticPressure2Init();
-    void staticPressure2Callback();
-
-    void imuInit();
-    void imuCallback();
-
-    // Sensors instances
-    Boardcore::LPS22DF* lps22df       = nullptr;
-    Boardcore::LPS28DFW* lps28dfw_1   = nullptr;
-    Boardcore::LPS28DFW* lps28dfw_2   = nullptr;
-    Boardcore::H3LIS331DL* h3lis331dl = nullptr;
-    Boardcore::LIS2MDL* lis2mdl       = nullptr;
-    Boardcore::UBXGPSSpi* ubxgps      = nullptr;
-    Boardcore::LSM6DSRX* lsm6dsrx     = nullptr;
-    Boardcore::ADS131M08* ads131m08   = nullptr;
-
-    // Can sensors
-    Boardcore::PitotData canPitot{0, 0, 0};
-    Boardcore::PressureData canCCPressure{0, 0};
-    Boardcore::PressureData canBottomTankPressure{0, 0};
-    Boardcore::PressureData canTopTankPressure{0, 0};
-    Boardcore::TemperatureData canTankTemperature{0, 0};
-    Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{};
-    Boardcore::CurrentData canMotorCurrent{};
-
-    // Fake processed sensors
-    RotatedIMU* imu                         = nullptr;
-    Boardcore::MPXH6400A* mpxh6400a         = nullptr;
-    Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr;
-    Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr;
-
-    // Magnetometer live calibration
-    Boardcore::SoftAndHardIronCalibration magCalibrator;
-    Boardcore::SixParametersCorrector magCalibration;
-    miosix::FastMutex calibrationMutex;
-
-    // Sensor manager
-    Boardcore::SensorManager* manager = nullptr;
-    Boardcore::SensorManager::SensorMap_t sensorMap;
-    Boardcore::TaskScheduler* scheduler = nullptr;
-
-    // Collection of lambdas to get the sensor init statuses
-    std::array<std::function<Boardcore::SensorInfo()>,
-               SensorsConfig::NUMBER_OF_SENSORS>
-        sensorsInit;
-    uint8_t sensorsId = 0;
-
-    // SD logger
-    Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance();
+    void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map);
+    void internalAdcCallback();
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+
+    std::atomic<bool> started{false};
+
+    std::unique_ptr<Boardcore::LPS22DF> lps22df;
+    std::unique_ptr<Boardcore::LPS28DFW> lps28dfw;
+    std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
+    std::unique_ptr<Boardcore::LIS2MDL> lis2mdl;
+    std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps;
+    std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx;
+    std::unique_ptr<Boardcore::ADS131M08> ads131m08;
+    std::unique_ptr<Boardcore::InternalADC> internalAdc;
+
+    std::unique_ptr<Boardcore::SensorManager> manager;
+
+    Boardcore::TaskScheduler &scheduler;
 };
+
 }  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main_old/Actuators/Actuators.cpp
similarity index 100%
rename from src/boards/Main/Actuators/Actuators.cpp
rename to src/boards/Main_old/Actuators/Actuators.cpp
diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main_old/Actuators/Actuators.h
similarity index 100%
rename from src/boards/Main/Actuators/Actuators.h
rename to src/boards/Main_old/Actuators/Actuators.h
diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.cpp
similarity index 100%
rename from src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
rename to src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.cpp
diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.h
similarity index 100%
rename from src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
rename to src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.h
diff --git a/src/boards/Main/BoardScheduler.cpp b/src/boards/Main_old/BoardScheduler.cpp
similarity index 100%
rename from src/boards/Main/BoardScheduler.cpp
rename to src/boards/Main_old/BoardScheduler.cpp
diff --git a/src/boards/Main/BoardScheduler.h b/src/boards/Main_old/BoardScheduler.h
similarity index 100%
rename from src/boards/Main/BoardScheduler.h
rename to src/boards/Main_old/BoardScheduler.h
diff --git a/src/boards/Main_old/Buses.h b/src/boards/Main_old/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..13552ec6be401c1d3be9ebd4456e4da4e94d3b9a
--- /dev/null
+++ b/src/boards/Main_old/Buses.h
@@ -0,0 +1,55 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+
+#pragma once
+
+#include <drivers/i2c/I2C.h>
+#include <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+namespace Main
+{
+class Buses : public Boardcore::Module
+{
+public:
+    Boardcore::SPIBus spi1;
+    Boardcore::SPIBus spi3;
+    Boardcore::SPIBus spi4;
+    Boardcore::SPIBus spi6;
+
+    Boardcore::I2C i2c1;
+
+    Boardcore::USART usart1;
+    Boardcore::USART usart2;
+    Boardcore::USART uart4;
+
+    Buses()
+        : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6),
+          i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(),
+               miosix::interfaces::i2c1::sda::getPin()),
+          usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200)
+    {
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main_old/CanHandler/CanHandler.cpp
similarity index 100%
rename from src/boards/Main/CanHandler/CanHandler.cpp
rename to src/boards/Main_old/CanHandler/CanHandler.cpp
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main_old/CanHandler/CanHandler.h
similarity index 100%
rename from src/boards/Main/CanHandler/CanHandler.h
rename to src/boards/Main_old/CanHandler/CanHandler.h
diff --git a/src/boards/Main/CanHandler/CanHandlerData.h b/src/boards/Main_old/CanHandler/CanHandlerData.h
similarity index 100%
rename from src/boards/Main/CanHandler/CanHandlerData.h
rename to src/boards/Main_old/CanHandler/CanHandlerData.h
diff --git a/src/boards/Main/Configs/ABKConfig.h b/src/boards/Main_old/Configs/ABKConfig.h
similarity index 100%
rename from src/boards/Main/Configs/ABKConfig.h
rename to src/boards/Main_old/Configs/ABKConfig.h
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main_old/Configs/ADAConfig.h
similarity index 100%
rename from src/boards/Main/Configs/ADAConfig.h
rename to src/boards/Main_old/Configs/ADAConfig.h
diff --git a/src/boards/Main/Configs/ActuatorsConfig.h b/src/boards/Main_old/Configs/ActuatorsConfig.h
similarity index 100%
rename from src/boards/Main/Configs/ActuatorsConfig.h
rename to src/boards/Main_old/Configs/ActuatorsConfig.h
diff --git a/src/boards/Main/Configs/AltitudeTriggerConfig.h b/src/boards/Main_old/Configs/AltitudeTriggerConfig.h
similarity index 100%
rename from src/boards/Main/Configs/AltitudeTriggerConfig.h
rename to src/boards/Main_old/Configs/AltitudeTriggerConfig.h
diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main_old/Configs/CanHandlerConfig.h
similarity index 100%
rename from src/boards/Main/Configs/CanHandlerConfig.h
rename to src/boards/Main_old/Configs/CanHandlerConfig.h
diff --git a/src/boards/Main/Configs/DeploymentConfig.h b/src/boards/Main_old/Configs/DeploymentConfig.h
similarity index 100%
rename from src/boards/Main/Configs/DeploymentConfig.h
rename to src/boards/Main_old/Configs/DeploymentConfig.h
diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main_old/Configs/FlightModeManagerConfig.h
similarity index 100%
rename from src/boards/Main/Configs/FlightModeManagerConfig.h
rename to src/boards/Main_old/Configs/FlightModeManagerConfig.h
diff --git a/src/boards/Main/Configs/FlightStatsRecorderConfig.h b/src/boards/Main_old/Configs/FlightStatsRecorderConfig.h
similarity index 100%
rename from src/boards/Main/Configs/FlightStatsRecorderConfig.h
rename to src/boards/Main_old/Configs/FlightStatsRecorderConfig.h
diff --git a/src/boards/Main/Configs/MEAConfig.h b/src/boards/Main_old/Configs/MEAConfig.h
similarity index 100%
rename from src/boards/Main/Configs/MEAConfig.h
rename to src/boards/Main_old/Configs/MEAConfig.h
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main_old/Configs/NASConfig.h
similarity index 100%
rename from src/boards/Main/Configs/NASConfig.h
rename to src/boards/Main_old/Configs/NASConfig.h
diff --git a/src/boards/Main/Configs/PinHandlerConfig.h b/src/boards/Main_old/Configs/PinHandlerConfig.h
similarity index 100%
rename from src/boards/Main/Configs/PinHandlerConfig.h
rename to src/boards/Main_old/Configs/PinHandlerConfig.h
diff --git a/src/boards/Main_old/Configs/RadioConfig.h b/src/boards/Main_old/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..42354f01172a5f62b245cef278681457b5e1681f
--- /dev/null
+++ b/src/boards/Main_old/Configs/RadioConfig.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+#pragma once
+
+#include <common/Mavlink.h>
+
+namespace Main
+{
+namespace RadioConfig
+{
+// Mavlink driver template parameters
+constexpr uint32_t RADIO_PKT_LENGTH     = 255;
+constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20;
+constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+
+constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250;
+
+constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50;
+constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10;
+
+constexpr uint8_t MAV_SYSTEM_ID = 171;
+constexpr uint8_t MAV_COMP_ID   = 96;
+
+constexpr uint32_t MAVLINK_QUEUE_SIZE = 10;
+
+}  // namespace RadioConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main_old/Configs/SensorsConfig.h b/src/boards/Main_old/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..0873209db35bdcdb5aa8b11ddad26dcceb0ba4c4
--- /dev/null
+++ b/src/boards/Main_old/Configs/SensorsConfig.h
@@ -0,0 +1,121 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+#pragma once
+
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+
+namespace Main
+{
+namespace SensorsConfig
+{
+constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4;
+constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100;
+
+constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4;
+constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100;
+constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR =
+    Boardcore::LPS28DFW::FS_1260;
+
+constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR =
+    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
+constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU =
+    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR =
+    Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+
+constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE =
+    Boardcore::LIS2MDL::MD_CONTINUOUS;
+constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ;
+constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5;
+
+constexpr uint8_t UBXGPS_SAMPLE_RATE = 5;
+
+constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU =
+    Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+
+constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS =
+    Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16;
+constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR =
+    Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416;
+constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+
+constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS =
+    Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000;
+constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR =
+    Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416;
+
+constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE =
+    Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION
+    LSM6DSRX_FIFO_TIMESTAMP_DECIMATION =
+        Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR
+    LSM6DSRX_FIFO_TEMPERATURE_BDR =
+        Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
+
+constexpr Boardcore::ADS131M08Defs::OversamplingRatio
+    ADS131M08_OVERSAMPLING_RATIO =
+        Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true;
+
+constexpr uint32_t LPS22DF_PERIOD         = 20;  // [ms] 50Hz
+constexpr uint32_t LPS28DFW_PERIOD        = 20;  // [ms] 50Hz
+constexpr uint32_t H3LIS331DL_PERIOD      = 10;  // [ms] 100Hz
+constexpr uint32_t LIS2MDL_PERIOD         = 10;  // [ms] 100Hz
+constexpr uint32_t UBXGPS_PERIOD          = 1000 / UBXGPS_SAMPLE_RATE;  // [ms]
+constexpr uint32_t LSM6DSRX_PERIOD        = 20;  // [ms] 50Hz
+constexpr uint32_t ADS131M08_PERIOD       = 10;  // [ms] 100Hz
+constexpr uint32_t IMU_PERIOD             = 20;  // [ms] Fake processed IMU 50Hz
+constexpr uint32_t MAG_CALIBRATION_PERIOD = 20;  // [ms] 50Hz
+
+// ADC sensors configs
+constexpr float ADC_VOLTAGE_RANGE = 1.2f;  // [V] Voltage reading range
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_3;
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
+
+// ADC Voltage divider
+constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR =
+    (20.f / 2.4f) +
+    1;  // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider
+constexpr float CURRENT_CONVERSION_FACTOR =
+    (20.f / 4.f) / (12.f / (12.f + 33.f));
+constexpr float CURRENT_OFFSET = 0.133333333f;  // V in ADC
+
+// Calibration samples
+constexpr unsigned int CALIBRATION_SAMPLES = 20;
+constexpr unsigned int CALIBRATION_PERIOD  = 100;
+
+constexpr unsigned int NUMBER_OF_SENSORS = 8;
+
+}  // namespace SensorsConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.cpp
similarity index 100%
rename from src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
rename to src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.cpp
diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.h
similarity index 100%
rename from src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h
rename to src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.h
diff --git a/src/boards/Main/PinHandler/PinData.h b/src/boards/Main_old/PinHandler/PinData.h
similarity index 100%
rename from src/boards/Main/PinHandler/PinData.h
rename to src/boards/Main_old/PinHandler/PinData.h
diff --git a/src/boards/Main/PinHandler/PinHandler.cpp b/src/boards/Main_old/PinHandler/PinHandler.cpp
similarity index 100%
rename from src/boards/Main/PinHandler/PinHandler.cpp
rename to src/boards/Main_old/PinHandler/PinHandler.cpp
diff --git a/src/boards/Main/PinHandler/PinHandler.h b/src/boards/Main_old/PinHandler/PinHandler.h
similarity index 100%
rename from src/boards/Main/PinHandler/PinHandler.h
rename to src/boards/Main_old/PinHandler/PinHandler.h
diff --git a/src/boards/Main_old/Radio/Radio.cpp b/src/boards/Main_old/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f3c6b40f4bd6076d4cd71c8670b49f5cf9824282
--- /dev/null
+++ b/src/boards/Main_old/Radio/Radio.cpp
@@ -0,0 +1,497 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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 <Main/Actuators/Actuators.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/Buses.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <events/EventBroker.h>
+#include <radio/SX1278/SX1278Frontends.h>
+
+#include <Eigen/Core>
+
+using namespace Boardcore;
+using namespace Common;
+using namespace Eigen;
+
+#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+namespace Main
+{
+
+Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
+
+bool Radio::start()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Config the transceiver
+    SX1278Fsk::Config config;
+    config.freq_rf    = 419000000;
+    config.freq_dev   = 50000;
+    config.bitrate    = 48000;
+    config.rx_bw      = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
+    config.afc_bw     = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
+    config.ocp        = 120;
+    config.power      = 13;
+    config.shaping    = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0;
+    config.dc_free    = Boardcore::SX1278Fsk::Config::DcFree::WHITENING;
+    config.enable_crc = false;
+
+    std::unique_ptr<SX1278::ISX1278Frontend> frontend =
+        std::make_unique<Skyward433Frontend>();
+
+    transceiver = new SX1278Fsk(
+        modules.get<Buses>()->spi6, miosix::radio::cs::getPin(),
+        miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(),
+        miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64,
+        std::move(frontend));
+
+    // Config the radio
+    SX1278Fsk::Error error = transceiver->init(config);
+
+    // Add periodic telemetry send task
+    uint8_t result1 =
+        scheduler->addTask([&]() { this->sendPeriodicMessage(); },
+                           RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
+                           TaskScheduler::Policy::RECOVER);
+    uint8_t result2 = scheduler->addTask(
+        [&]()
+        {
+            this->enqueueMsg(
+                modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0));
+        },
+        RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2,
+        TaskScheduler::Policy::RECOVER);
+
+    // Config mavDriver
+    mavDriver = new MavDriver(
+        transceiver,
+        [=](MavDriver*, const mavlink_message_t& msg)
+        { this->handleMavlinkMessage(msg); },
+        RadioConfig::RADIO_SLEEP_AFTER_SEND,
+        RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
+
+    // Check radio failure
+    if (error != SX1278Fsk::Error::NONE)
+    {
+        return false;
+    }
+
+    // Start the mavlink driver thread
+    return mavDriver->start() && result1 != 0 && result2 != 0;
+}
+
+void Radio::sendAck(const mavlink_message_t& msg)
+{
+    mavlink_message_t ackMsg;
+    mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+                            RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid,
+                            msg.seq);
+    enqueueMsg(ackMsg);
+}
+
+void Radio::sendNack(const mavlink_message_t& msg)
+{
+    mavlink_message_t nackMsg;
+    mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+                             RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
+                             msg.seq);
+    enqueueMsg(nackMsg);
+}
+
+void Radio::logStatus() {}
+
+bool Radio::isStarted()
+{
+    return mavDriver->isStarted() && scheduler->isRunning();
+}
+
+void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    switch (msg.msgid)
+    {
+        case MAVLINK_MSG_ID_PING_TC:
+        {
+            // Do nothing, just add the ack to the queue
+            break;
+        }
+        case MAVLINK_MSG_ID_COMMAND_TC:
+        {
+            // Let the handle command reply to the message
+            return handleCommand(msg);
+        }
+        case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
+        {
+            SystemTMList tmId = static_cast<SystemTMList>(
+                mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
+
+            if (tmId == MAV_PIN_OBS_ID)
+            {
+                for (uint8_t i = 0; i < PinHandler::PIN_END; i++)
+                {
+                    mavlink_message_t msg;
+                    mavlink_pin_tm_t tm;
+
+                    PinData data = modules.get<PinHandler>()->getPinData(
+                        static_cast<PinHandler::PinList>(i));
+
+                    tm.changes_counter       = data.changesCount;
+                    tm.current_state         = tm.current_state;
+                    tm.last_change_timestamp = tm.last_change_timestamp;
+                    tm.pin_id                = tm.pin_id;
+                    tm.timestamp             = TimestampTimer::getTimestamp();
+
+                    mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                              RadioConfig::MAV_COMP_ID, &msg,
+                                              &tm);
+                    enqueueMsg(msg);
+                }
+            }
+            else if (tmId == MAV_SENSORS_STATE_ID)
+            {
+                auto sensorsState = modules.get<Sensors>()->getSensorInfo();
+
+                for (uint8_t i = 0; i < sensorsState.size(); i++)
+                {
+                    mavlink_message_t msg;
+                    mavlink_sensor_state_tm_t tm;
+
+                    strcpy(tm.sensor_name, sensorsState.at(i).id.c_str());
+                    tm.state = sensorsState.at(i).isInitialized;
+
+                    mavlink_msg_sensor_state_tm_encode(
+                        RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
+                        &msg, &tm);
+                    enqueueMsg(msg);
+                }
+            }
+            else
+            {
+                // Add to the queue the respose
+                mavlink_message_t response =
+                    modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid,
+                                                              msg.seq);
+                // Add the response to the queue
+                enqueueMsg(response);
+
+                // Check if the TM repo answered with a NACK. If so the function
+                // must return to avoid sending a default ack
+                if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+                {
+                    return;
+                }
+            }
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
+        {
+            SensorsTMList sensorId = static_cast<SensorsTMList>(
+                mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
+            mavlink_message_t response =
+                modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid,
+                                                           msg.seq);
+            enqueueMsg(response);
+
+            // If the response is a nack the method returns
+            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+            {
+                return;
+            }
+            break;
+        }
+        case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
+        {
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
+            mavlink_message_t response =
+                modules.get<TMRepository>()->packServoTm(servo, msg.msgid,
+                                                         msg.seq);
+            enqueueMsg(response);
+
+            // If the response is a nack the method returns
+            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+            {
+                return;
+            }
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
+            float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // If the state is test mode, the servo is set to the correct angle
+            modules.get<Actuators>()->setServoPosition(servoId, position);
+            break;
+        }
+        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // If the state is test mode, the wiggle is done
+            modules.get<Actuators>()->wiggleServo(servoId);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_RESET_SERVO_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_reset_servo_tc_get_servo_id(&msg));
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // Set the servo position to 0
+            modules.get<Actuators>()->setServoPosition(servoId, 0);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
+        {
+            float altitude =
+                mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
+
+            modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
+        {
+            float altitude =
+                mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
+
+            modules.get<ADAController>()->setReferenceAltitude(altitude);
+            modules.get<NASController>()->setReferenceAltitude(altitude);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
+        {
+            float temperature =
+                mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
+
+            modules.get<ADAController>()->setReferenceTemperature(temperature);
+            modules.get<NASController>()->setReferenceTemperature(temperature);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_COORDINATES_TC:
+        {
+            float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
+            float longitude =
+                mavlink_msg_set_coordinates_tc_get_longitude(&msg);
+
+            Vector2f coordinates{latitude, longitude};
+
+            modules.get<NASController>()->setCoordinates(coordinates);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_ORIENTATION_TC:
+        {
+            float yaw   = mavlink_msg_set_orientation_tc_get_yaw(&msg);
+            float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg);
+            float roll  = mavlink_msg_set_orientation_tc_get_roll(&msg);
+
+            modules.get<NASController>()->setOrientation(yaw, pitch, roll);
+            break;
+        }
+        default:
+        {
+            LOG_DEBUG(logger, "Received message is not of a known type");
+            return sendNack(msg);
+        }
+    }
+
+    // At the end send the ack message
+    sendAck(msg);
+}
+
+void Radio::handleCommand(const mavlink_message_t& msg)
+{
+    MavCommandList commandId = static_cast<MavCommandList>(
+        mavlink_msg_command_tc_get_command_id(&msg));
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Create the map between the commands and the corresponding events
+    static const std::map<MavCommandList, Events> commandToEvent{
+        {MAV_CMD_ARM, TMTC_ARM},
+        {MAV_CMD_DISARM, TMTC_DISARM},
+        {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
+        {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
+        {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
+        {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
+        {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
+        {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
+        {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
+        {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
+        {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE},
+        {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE},
+        {MAV_CMD_START_RECORDING, TMTC_START_RECORDING},
+        {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING},
+    };
+
+    switch (commandId)
+    {
+        case MAV_CMD_SAVE_CALIBRATION:
+        {
+            // Save the sensor calibration and adopt it
+            bool result = modules.get<Sensors>()->writeMagCalibration();
+
+            if (!result)
+            {
+                return sendNack(msg);
+            }
+
+            break;
+        }
+        case MAV_CMD_START_LOGGING:
+        {
+            bool result = Logger::getInstance().start();
+
+            // In case the logger is not started send to GS the result
+            if (!result)
+            {
+                return sendNack(msg);
+            }
+            break;
+        }
+        case MAV_CMD_STOP_LOGGING:
+        {
+            Logger::getInstance().stop();
+            break;
+        }
+        default:
+        {
+            // If the command is not a particular one, look for it inside the
+            // map
+            auto it = commandToEvent.find(commandId);
+
+            if (it != commandToEvent.end())
+            {
+                EventBroker::getInstance().post(it->second, TOPIC_TMTC);
+            }
+            else
+            {
+                return sendNack(msg);
+            }
+        }
+    }
+    // Acknowledge the message
+    sendAck(msg);
+}
+
+void Radio::sendPeriodicMessage()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Send all the queue messages
+    {
+        Lock<FastMutex> lock(queueMutex);
+
+        for (uint8_t i = 0; i < messageQueueIndex; i++)
+        {
+            mavDriver->enqueueMsg(messageQueue[i]);
+        }
+
+        // Reset the index
+        messageQueueIndex = 0;
+    }
+
+    mavDriver->enqueueMsg(
+        modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0));
+    mavDriver->enqueueMsg(
+        modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0));
+}
+
+void Radio::enqueueMsg(const mavlink_message_t& msg)
+{
+    {
+        Lock<FastMutex> lock(queueMutex);
+
+        // Insert the message inside the queue only if there is enough space
+        if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE)
+        {
+            messageQueue[messageQueueIndex] = msg;
+            messageQueueIndex++;
+        }
+    }
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main_old/Radio/Radio.h b/src/boards/Main_old/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c1fd4cde31fcbde4b190b31c0e3c56291b728e6
--- /dev/null
+++ b/src/boards/Main_old/Radio/Radio.h
@@ -0,0 +1,102 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+#pragma once
+
+#include <Main/Configs/RadioConfig.h>
+#include <common/Mavlink.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SX1278/SX1278Fsk.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
+                                           RadioConfig::RADIO_OUT_QUEUE_SIZE,
+                                           RadioConfig::RADIO_MAV_MSG_LENGTH>;
+
+class Radio : public Boardcore::Module
+{
+public:
+    Radio(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the MavlinkDriver
+     */
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Sends via radio an acknowledge message about the parameter passed
+     * message
+     */
+    void sendAck(const mavlink_message_t& msg);
+
+    /**
+     * @brief Sends via radio an non-acknowledge message about the parameter
+     * passed message
+     */
+    void sendNack(const mavlink_message_t& msg);
+
+    /**
+     * @brief Saves the MavlinkDriver and transceiver status
+     */
+    void logStatus();
+
+    /**
+     * @brief Returns if the radio module is correctly started
+     */
+    bool isStarted();
+
+    Boardcore::SX1278Fsk* transceiver = nullptr;
+    MavDriver* mavDriver              = nullptr;
+
+private:
+    /**
+     * @brief Called by the MavlinkDriver when a message is received
+     */
+    void handleMavlinkMessage(const mavlink_message_t& msg);
+
+    /**
+     * @brief Called by the handleMavlinkMessage to handle a command message
+     */
+    void handleCommand(const mavlink_message_t& msg);
+
+    /**
+     * @brief Sends the periodic telemetry
+     */
+    void sendPeriodicMessage();
+
+    /**
+     * @brief Inserts the mavlink message into the queue
+     */
+    void enqueueMsg(const mavlink_message_t& msg);
+
+    // Messages queue
+    mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE];
+    uint32_t messageQueueIndex = 0;
+    miosix::FastMutex queueMutex;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
+    Boardcore::TaskScheduler* scheduler = nullptr;
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.cpp
similarity index 100%
rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.cpp
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.h
similarity index 100%
rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h
rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.h
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMUData.h
similarity index 100%
rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h
rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMUData.h
diff --git a/src/boards/Main_old/Sensors/Sensors.cpp b/src/boards/Main_old/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2333ee4e1a3748105e91febe1245ce29d9425bfd
--- /dev/null
+++ b/src/boards/Main_old/Sensors/Sensors.cpp
@@ -0,0 +1,735 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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 "Sensors.h"
+
+#include <Main/Buses.h>
+#include <Main/Configs/SensorsConfig.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace Boardcore;
+using namespace std;
+using namespace Main::SensorsConfig;
+namespace Main
+{
+LPS22DFData Sensors::getLPS22DFLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_1LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_2LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{};
+}
+H3LIS331DLData Sensors::getH3LIS331DLLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
+                                 : H3LIS331DLData{};
+}
+LIS2MDLData Sensors::getLIS2MDLLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
+}
+UBXGPSData Sensors::getGPSLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{};
+}
+LSM6DSRXData Sensors::getLSM6DSRXLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+}
+ADS131M08Data Sensors::getADS131M08LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+PitotData Sensors::getPitotLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canPitot;
+}
+PressureData Sensors::getCCPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canCCPressure;
+}
+PressureData Sensors::getBottomTankPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canBottomTankPressure;
+}
+PressureData Sensors::getTopTankPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canTopTankPressure;
+}
+TemperatureData Sensors::getTankTemperatureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canTankTemperature;
+}
+BatteryVoltageSensorData Sensors::getMotorBatteryVoltage()
+{
+    miosix::PauseKernelLock lock;
+    return canMotorBatteryVoltage;
+}
+CurrentData Sensors::getMotorCurrent()
+{
+    miosix::PauseKernelLock lock;
+    return canMotorCurrent;
+}
+
+// Processed Getters
+BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    BatteryVoltageSensorData data;
+
+    // Populate the data
+    data.voltageTimestamp = sample.timestamp;
+    data.channelId        = 1;
+    data.voltage          = sample.voltage[1];
+    data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
+    return data;
+}
+
+BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    BatteryVoltageSensorData data;
+
+    // Populate the data
+    data.voltageTimestamp = sample.timestamp;
+    data.channelId        = 7;
+    data.voltage          = sample.voltage[7];
+    data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
+    return data;
+}
+
+CurrentData Sensors::getCurrentLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    CurrentData data;
+
+    // Populate the data
+    data.currentTimestamp = sample.timestamp;
+    data.current =
+        (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR;
+    return data;
+}
+
+MPXH6400AData Sensors::getDeploymentPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{};
+}
+
+HSCMRNN015PAData Sensors::getStaticPressure1LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample()
+                                     : HSCMRNN015PAData{};
+}
+
+HSCMRNN015PAData Sensors::getStaticPressure2LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample()
+                                     : HSCMRNN015PAData{};
+}
+
+RotatedIMUData Sensors::getIMULastSample()
+{
+    miosix::PauseKernelLock lock;
+    return imu != nullptr ? imu->getLastSample() : RotatedIMUData{};
+}
+
+MagnetometerData Sensors::getCalibratedMagnetometerLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    MagnetometerData lastSample = getLIS2MDLLastSample();
+    MagnetometerData result;
+
+    // Correct the result and copy the timestamp
+    {
+        miosix::Lock<FastMutex> l(calibrationMutex);
+        result =
+            static_cast<MagnetometerData>(magCalibration.correct(lastSample));
+    }
+
+    result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp;
+    return result;
+}
+
+void Sensors::setPitot(PitotData data)
+{
+    miosix::PauseKernelLock lock;
+    canPitot           = data;
+    canPitot.timestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setCCPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canCCPressure                   = data;
+    canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setBottomTankPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canBottomTankPressure                   = data;
+    canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setTopTankPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canTopTankPressure                   = data;
+    canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setTankTemperature(TemperatureData data)
+{
+    miosix::PauseKernelLock lock;
+    canTankTemperature                      = data;
+    canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data)
+{
+    miosix::PauseKernelLock lock;
+    canMotorBatteryVoltage.batVoltage       = data.batVoltage;
+    canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp();
+}
+void Sensors::setMotorCurrent(CurrentData data)
+{
+    miosix::PauseKernelLock lock;
+    canMotorCurrent.current          = data.current;
+    canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp();
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+
+bool Sensors::start()
+{
+    // Read the magnetometer calibration from predefined file
+    magCalibration.fromFile("/sd/magCalibration.csv");
+
+    // Init all the sensors
+    lps22dfInit();
+    lps28dfw_1Init();
+    // lps28dfw_2Init();
+    h3lis331dlInit();
+    lis2mdlInit();
+    ubxgpsInit();
+    lsm6dsrxInit();
+    ads131m08Init();
+    deploymentPressureInit();
+    staticPressure1Init();
+    staticPressure2Init();
+    imuInit();
+
+    // Add the magnetometer calibration to the scheduler
+    size_t result = scheduler->addTask(
+        [&]()
+        {
+            // Gather the last sample data
+            MagnetometerData lastSample = getLIS2MDLLastSample();
+
+            // Feed the data to the calibrator inside a protected area.
+            // Contention is not high and the use of a mutex is suitable to
+            // avoid pausing the kernel for this calibration operation
+            {
+                miosix::Lock<FastMutex> l(calibrationMutex);
+                magCalibrator.feed(lastSample);
+            }
+        },
+        MAG_CALIBRATION_PERIOD);
+
+    // Create sensor manager with populated map and configured scheduler
+    manager = new SensorManager(sensorMap, scheduler);
+    return manager->start() && result != 0;
+}
+
+void Sensors::stop() { manager->stop(); }
+
+bool Sensors::isStarted()
+{
+    return manager->areAllSensorsInitialized() && scheduler->isRunning();
+}
+
+void Sensors::calibrate()
+{
+    // Create the stats to calibrate the barometers
+    Stats lps28dfw1Stats;
+    // Stats lps28dfw2Stats;
+    Stats staticPressure1Stats;
+    Stats staticPressure2Stats;
+    Stats deploymentPressureStats;
+
+    // Add N samples to the stats
+    for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++)
+    {
+        lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure);
+        // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure);
+        staticPressure1Stats.add(getStaticPressure1LastSample().pressure);
+        staticPressure2Stats.add(getStaticPressure2LastSample().pressure);
+        deploymentPressureStats.add(getDeploymentPressureLastSample().pressure);
+
+        // Delay for the expected period
+        miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD);
+    }
+
+    // Compute the difference between the mean value from LPS28DFW
+    float reference = lps28dfw1Stats.getStats().mean;
+
+    hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean -
+                                 reference);
+    hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean -
+                                 reference);
+    mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean -
+                            reference);
+
+    // Log the offsets
+    SensorsCalibrationParameter cal{};
+    cal.timestamp         = TimestampTimer::getTimestamp();
+    cal.offsetStatic1     = staticPressure1Stats.getStats().mean - reference;
+    cal.offsetStatic2     = staticPressure2Stats.getStats().mean - reference;
+    cal.offsetDeployment  = deploymentPressureStats.getStats().mean - reference;
+    cal.referencePressure = reference;
+
+    Logger::getInstance().log(cal);
+}
+
+bool Sensors::writeMagCalibration()
+{
+    // Compute the calibration result in protected area
+    {
+        miosix::Lock<FastMutex> l(calibrationMutex);
+        SixParametersCorrector cal = magCalibrator.computeResult();
+
+        // Check result validity
+        if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) &&
+            !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) &&
+            !isnan(cal.getA()[1]) && !isnan(cal.getA()[2]))
+        {
+            magCalibration = cal;
+
+            // Save the calibration to the calibration file
+            return magCalibration.toFile("/sd/magCalibration.csv");
+        }
+        return false;
+    }
+}
+
+std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+Sensors::getSensorInfo()
+{
+    std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState;
+    for (size_t i = 0; i < sensorsInit.size(); i++)
+    {
+        // Check wether the lambda is existent
+        if (sensorsInit[i])
+        {
+            sensorState[i] = sensorsInit[i]();
+        }
+    }
+    return sensorState;
+}
+
+void Sensors::lps22dfInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = LPS22DF::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Configure the device
+    LPS22DF::Config sensorConfig;
+    sensorConfig.avg = LPS22DF_AVG;
+    sensorConfig.odr = LPS22DF_ODR;
+
+    // Create sensor instance with configured parameters
+    lps22df = new LPS22DF(modules.get<Buses>()->spi3,
+                          miosix::sensors::LPS22DF::cs::getPin(), config,
+                          sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS22DF", LPS22DF_PERIOD,
+                    bind(&Sensors::lps22dfCallback, this));
+    sensorMap.emplace(make_pair(lps22df, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps22df); };
+}
+void Sensors::lps28dfw_1Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD,
+                    bind(&Sensors::lps28dfw_1Callback, this));
+    sensorMap.emplace(make_pair(lps28dfw_1, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps28dfw_1); };
+}
+void Sensors::lps28dfw_2Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD,
+                    bind(&Sensors::lps28dfw_2Callback, this));
+    sensorMap.emplace(make_pair(lps28dfw_2, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps28dfw_2); };
+}
+void Sensors::h3lis331dlInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Create sensor instance with configured parameters
+    h3lis331dl = new H3LIS331DL(
+        modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(),
+        config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD,
+                    bind(&Sensors::h3lis331dlCallback, this));
+    sensorMap.emplace(make_pair(h3lis331dl, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(h3lis331dl); };
+}
+void Sensors::lis2mdlInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = LIS2MDL::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Configure the sensor
+    LIS2MDL::Config sensorConfig;
+    sensorConfig.deviceMode         = LIS2MDL_OPERATIVE_MODE;
+    sensorConfig.odr                = LIS2MDL_ODR;
+    sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER;
+
+    // Create sensor instance with configured parameters
+    lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3,
+                          miosix::sensors::LIS2MDL::cs::getPin(), config,
+                          sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LIS2MDL", LIS2MDL_PERIOD,
+                    bind(&Sensors::lis2mdlCallback, this));
+    sensorMap.emplace(make_pair(lis2mdl, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lis2mdl); };
+}
+
+void Sensors::ubxgpsInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_64;
+
+    // Create sensor instance with configured parameters
+    ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4,
+                           miosix::sensors::GPS::cs::getPin(), config, 5);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("UBXGPS", UBXGPS_PERIOD,
+                    bind(&Sensors::ubxgpsCallback, this));
+    sensorMap.emplace(make_pair(ubxgps, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(ubxgps); };
+}
+
+void Sensors::lsm6dsrxInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the SPI
+    SPIBusConfig config;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+    config.mode         = SPI::Mode::MODE_0;
+
+    // Configure the sensor
+    LSM6DSRXConfig sensorConfig;
+    sensorConfig.bdu = LSM6DSRX_BDU;
+
+    // Accelerometer
+    sensorConfig.fsAcc     = LSM6DSRX_ACC_FS;
+    sensorConfig.odrAcc    = LSM6DSRX_ACC_ODR;
+    sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE;
+
+    // Gyroscope
+    sensorConfig.fsGyr     = LSM6DSRX_GYR_FS;
+    sensorConfig.odrGyr    = LSM6DSRX_GYR_ODR;
+    sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE;
+
+    // Fifo
+    sensorConfig.fifoMode                = LSM6DSRX_FIFO_MODE;
+    sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION;
+    sensorConfig.fifoTemperatureBdr      = LSM6DSRX_FIFO_TEMPERATURE_BDR;
+
+    // Create sensor instance with configured parameters
+    lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1,
+                            miosix::sensors::LSM6DSRX::cs::getPin(), config,
+                            sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD,
+                    bind(&Sensors::lsm6dsrxCallback, this));
+    sensorMap.emplace(make_pair(lsm6dsrx, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lsm6dsrx); };
+}
+
+void Sensors::ads131m08Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the SPI
+    SPIBusConfig config;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+
+    // Configure the device
+    ADS131M08::Config sensorConfig;
+    sensorConfig.oversamplingRatio     = ADS131M08_OVERSAMPLING_RATIO;
+    sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE;
+
+    // Create the sensor instance with configured parameters
+    ads131m08 = new ADS131M08(modules.get<Buses>()->spi4,
+                              miosix::sensors::ADS131::cs::getPin(), config,
+                              sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("ADS131M08", ADS131M08_PERIOD,
+                    bind(&Sensors::ads131m08Callback, this));
+    sensorMap.emplace(make_pair(ads131m08, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(ads131m08); };
+}
+
+void Sensors::deploymentPressureInit()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_DPL);
+    };
+
+    // Create the sensor instance with created function
+    mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("MPXH6400A", ADS131M08_PERIOD,
+                    bind(&Sensors::deploymentPressureCallback, this));
+    sensorMap.emplace(make_pair(mpxh6400a, info));
+}
+
+void Sensors::staticPressure1Init()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_STATIC_1);
+    };
+
+    // Create the sensor instance with created function
+    hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD,
+                    bind(&Sensors::staticPressure1Callback, this));
+    sensorMap.emplace(make_pair(hscmrnn015pa_1, info));
+}
+
+void Sensors::staticPressure2Init()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_STATIC_2);
+    };
+
+    // Create the sensor instance with created function
+    hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD,
+                    bind(&Sensors::staticPressure2Callback, this));
+    sensorMap.emplace(make_pair(hscmrnn015pa_2, info));
+}
+
+void Sensors::imuInit()
+{
+    // Register the IMU as the fake sensor, passing as parameters the methods to
+    // retrieve real data. The sensor is not synchronized, but the sampling
+    // thread is always the same.
+    imu = new RotatedIMU(
+        bind(&LSM6DSRX::getLastSample, lsm6dsrx),
+        bind(&Sensors::getCalibratedMagnetometerLastSample, this),
+        bind(&LSM6DSRX::getLastSample, lsm6dsrx));
+
+    // Invert the Y axis on the magnetometer
+    Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}};
+    imu->addMagTransformation(m);
+    imu->addAccTransformation(imu->rotateAroundX(45));
+    imu->addGyroTransformation(imu->rotateAroundX(45));
+    imu->addMagTransformation(imu->rotateAroundX(45));
+
+    // Emplace the sensor inside the map
+    SensorInfo info("RotatedIMU", IMU_PERIOD,
+                    bind(&Sensors::imuCallback, this));
+    sensorMap.emplace(make_pair(imu, info));
+}
+
+void Sensors::lps22dfCallback()
+{
+    LPS22DFData lastSample = lps22df->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_1Callback()
+{
+    LPS28DFW_1Data lastSample =
+        static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_2Callback()
+{
+    LPS28DFW_2Data lastSample =
+        static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::h3lis331dlCallback()
+{
+    H3LIS331DLData lastSample = h3lis331dl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lis2mdlCallback()
+{
+    LIS2MDLData lastSample = lis2mdl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::ubxgpsCallback()
+{
+    UBXGPSData lastSample = ubxgps->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lsm6dsrxCallback()
+{
+    auto& fifo        = lsm6dsrx->getLastFifo();
+    uint16_t fifoSize = lsm6dsrx->getLastFifoSize();
+
+    // For every instance inside the fifo log the sample
+    for (uint16_t i = 0; i < fifoSize; i++)
+    {
+        Logger::getInstance().log(fifo.at(i));
+    }
+}
+void Sensors::ads131m08Callback()
+{
+    ADS131M08Data lastSample = ads131m08->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::deploymentPressureCallback()
+{
+    MPXH6400AData lastSample = mpxh6400a->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::staticPressure1Callback()
+{
+    HSCMRNN015PA_1Data lastSample =
+        static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::staticPressure2Callback()
+{
+    HSCMRNN015PA_2Data lastSample =
+        static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::imuCallback()
+{
+    RotatedIMUData lastSample = imu->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main_old/Sensors/Sensors.h b/src/boards/Main_old/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..44ad52749f84cc0a750388f1581969611e76cdf2
--- /dev/null
+++ b/src/boards/Main_old/Sensors/Sensors.h
@@ -0,0 +1,202 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+#pragma once
+
+#include <Main/Configs/SensorsConfig.h>
+#include <Main/Sensors/RotatedIMU/RotatedIMU.h>
+#include <Main/Sensors/SensorsData.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/SensorData.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <sensors/analog/BatteryVoltageSensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h>
+#include <sensors/analog/pressure/nxp/MPXH6400A.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+#include <stdint.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class Sensors : public Boardcore::Module
+{
+public:
+    explicit Sensors(Boardcore::TaskScheduler* sched);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Stops the sensor manager
+     * @warning Stops the passed scheduler
+     */
+    void stop();
+
+    /**
+     * @brief Returns if all the sensors are started successfully
+     */
+    bool isStarted();
+
+    /**
+     * @brief Calibrates the sensors with an offset
+     */
+    void calibrate();
+
+    /**
+     * @brief Takes the result of the live magnetometer calibration and applies
+     * it to the current calibration + writes it in the csv file
+     */
+    bool writeMagCalibration();
+
+    // Sensor getters
+    Boardcore::LPS22DFData getLPS22DFLastSample();
+    Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
+    Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
+    Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+    Boardcore::LIS2MDLData getLIS2MDLLastSample();
+    Boardcore::UBXGPSData getGPSLastSample();
+    Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+    Boardcore::ADS131M08Data getADS131M08LastSample();
+
+    // Processed getters
+    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+    Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
+    Boardcore::CurrentData getCurrentLastSample();
+    Boardcore::MPXH6400AData getDeploymentPressureLastSample();
+    Boardcore::HSCMRNN015PAData getStaticPressure1LastSample();
+    Boardcore::HSCMRNN015PAData getStaticPressure2LastSample();
+    RotatedIMUData getIMULastSample();
+    Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
+
+    // CAN fake sensors setters
+    void setPitot(Boardcore::PitotData data);
+    void setCCPressure(Boardcore::PressureData data);
+    void setBottomTankPressure(Boardcore::PressureData data);
+    void setTopTankPressure(Boardcore::PressureData data);
+    void setTankTemperature(Boardcore::TemperatureData data);
+    void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data);
+    void setMotorCurrent(Boardcore::CurrentData data);
+
+    // CAN fake sensors getters
+    Boardcore::PitotData getPitotLastSample();
+    Boardcore::PressureData getCCPressureLastSample();
+    Boardcore::PressureData getBottomTankPressureLastSample();
+    Boardcore::PressureData getTopTankPressureLastSample();
+    Boardcore::TemperatureData getTankTemperatureLastSample();
+    Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage();
+    Boardcore::CurrentData getMotorCurrent();
+
+    // Returns the sensors statuses
+    std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+    getSensorInfo();
+
+private:
+    // Init and callbacks methods
+    void lps22dfInit();
+    void lps22dfCallback();
+
+    void lps28dfw_1Init();
+    void lps28dfw_1Callback();
+
+    void lps28dfw_2Init();
+    void lps28dfw_2Callback();
+
+    void h3lis331dlInit();
+    void h3lis331dlCallback();
+
+    void lis2mdlInit();
+    void lis2mdlCallback();
+
+    void ubxgpsInit();
+    void ubxgpsCallback();
+
+    void lsm6dsrxInit();
+    void lsm6dsrxCallback();
+
+    void ads131m08Init();
+    void ads131m08Callback();
+
+    void deploymentPressureInit();
+    void deploymentPressureCallback();
+
+    void staticPressure1Init();
+    void staticPressure1Callback();
+
+    void staticPressure2Init();
+    void staticPressure2Callback();
+
+    void imuInit();
+    void imuCallback();
+
+    // Sensors instances
+    Boardcore::LPS22DF* lps22df       = nullptr;
+    Boardcore::LPS28DFW* lps28dfw_1   = nullptr;
+    Boardcore::LPS28DFW* lps28dfw_2   = nullptr;
+    Boardcore::H3LIS331DL* h3lis331dl = nullptr;
+    Boardcore::LIS2MDL* lis2mdl       = nullptr;
+    Boardcore::UBXGPSSpi* ubxgps      = nullptr;
+    Boardcore::LSM6DSRX* lsm6dsrx     = nullptr;
+    Boardcore::ADS131M08* ads131m08   = nullptr;
+
+    // Can sensors
+    Boardcore::PitotData canPitot{0, 0, 0};
+    Boardcore::PressureData canCCPressure{0, 0};
+    Boardcore::PressureData canBottomTankPressure{0, 0};
+    Boardcore::PressureData canTopTankPressure{0, 0};
+    Boardcore::TemperatureData canTankTemperature{0, 0};
+    Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{};
+    Boardcore::CurrentData canMotorCurrent{};
+
+    // Fake processed sensors
+    RotatedIMU* imu                         = nullptr;
+    Boardcore::MPXH6400A* mpxh6400a         = nullptr;
+    Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr;
+    Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr;
+
+    // Magnetometer live calibration
+    Boardcore::SoftAndHardIronCalibration magCalibrator;
+    Boardcore::SixParametersCorrector magCalibration;
+    miosix::FastMutex calibrationMutex;
+
+    // Sensor manager
+    Boardcore::SensorManager* manager = nullptr;
+    Boardcore::SensorManager::SensorMap_t sensorMap;
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Collection of lambdas to get the sensor init statuses
+    std::array<std::function<Boardcore::SensorInfo()>,
+               SensorsConfig::NUMBER_OF_SENSORS>
+        sensorsInit;
+    uint8_t sensorsId = 0;
+
+    // SD logger
+    Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance();
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/SensorsData.h b/src/boards/Main_old/Sensors/SensorsData.h
similarity index 100%
rename from src/boards/Main/Sensors/SensorsData.h
rename to src/boards/Main_old/Sensors/SensorsData.h
diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.cpp b/src/boards/Main_old/StateMachines/ABKController/ABKController.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/ABKController/ABKController.cpp
rename to src/boards/Main_old/StateMachines/ABKController/ABKController.cpp
diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.h b/src/boards/Main_old/StateMachines/ABKController/ABKController.h
similarity index 100%
rename from src/boards/Main/StateMachines/ABKController/ABKController.h
rename to src/boards/Main_old/StateMachines/ABKController/ABKController.h
diff --git a/src/boards/Main/StateMachines/ABKController/ABKControllerData.h b/src/boards/Main_old/StateMachines/ABKController/ABKControllerData.h
similarity index 100%
rename from src/boards/Main/StateMachines/ABKController/ABKControllerData.h
rename to src/boards/Main_old/StateMachines/ABKController/ABKControllerData.h
diff --git a/src/boards/Main/StateMachines/ABKController/TrajectorySet.h b/src/boards/Main_old/StateMachines/ABKController/TrajectorySet.h
similarity index 100%
rename from src/boards/Main/StateMachines/ABKController/TrajectorySet.h
rename to src/boards/Main_old/StateMachines/ABKController/TrajectorySet.h
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main_old/StateMachines/ADAController/ADAController.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/ADAController/ADAController.cpp
rename to src/boards/Main_old/StateMachines/ADAController/ADAController.cpp
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main_old/StateMachines/ADAController/ADAController.h
similarity index 100%
rename from src/boards/Main/StateMachines/ADAController/ADAController.h
rename to src/boards/Main_old/StateMachines/ADAController/ADAController.h
diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main_old/StateMachines/ADAController/ADAControllerData.h
similarity index 100%
rename from src/boards/Main/StateMachines/ADAController/ADAControllerData.h
rename to src/boards/Main_old/StateMachines/ADAController/ADAControllerData.h
diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.cpp b/src/boards/Main_old/StateMachines/Deployment/Deployment.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/Deployment/Deployment.cpp
rename to src/boards/Main_old/StateMachines/Deployment/Deployment.cpp
diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.h b/src/boards/Main_old/StateMachines/Deployment/Deployment.h
similarity index 100%
rename from src/boards/Main/StateMachines/Deployment/Deployment.h
rename to src/boards/Main_old/StateMachines/Deployment/Deployment.h
diff --git a/src/boards/Main/StateMachines/Deployment/DeploymentData.h b/src/boards/Main_old/StateMachines/Deployment/DeploymentData.h
similarity index 100%
rename from src/boards/Main/StateMachines/Deployment/DeploymentData.h
rename to src/boards/Main_old/StateMachines/Deployment/DeploymentData.h
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.cpp
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.h
similarity index 100%
rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.h
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManagerData.h
similarity index 100%
rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManagerData.h
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main_old/StateMachines/MEAController/MEAController.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/MEAController/MEAController.cpp
rename to src/boards/Main_old/StateMachines/MEAController/MEAController.cpp
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main_old/StateMachines/MEAController/MEAController.h
similarity index 100%
rename from src/boards/Main/StateMachines/MEAController/MEAController.h
rename to src/boards/Main_old/StateMachines/MEAController/MEAController.h
diff --git a/src/boards/Main/StateMachines/MEAController/MEAControllerData.h b/src/boards/Main_old/StateMachines/MEAController/MEAControllerData.h
similarity index 100%
rename from src/boards/Main/StateMachines/MEAController/MEAControllerData.h
rename to src/boards/Main_old/StateMachines/MEAController/MEAControllerData.h
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main_old/StateMachines/NASController/NASController.cpp
similarity index 100%
rename from src/boards/Main/StateMachines/NASController/NASController.cpp
rename to src/boards/Main_old/StateMachines/NASController/NASController.cpp
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main_old/StateMachines/NASController/NASController.h
similarity index 100%
rename from src/boards/Main/StateMachines/NASController/NASController.h
rename to src/boards/Main_old/StateMachines/NASController/NASController.h
diff --git a/src/boards/Main/StateMachines/NASController/NASControllerData.h b/src/boards/Main_old/StateMachines/NASController/NASControllerData.h
similarity index 100%
rename from src/boards/Main/StateMachines/NASController/NASControllerData.h
rename to src/boards/Main_old/StateMachines/NASController/NASControllerData.h
diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main_old/TMRepository/TMRepository.cpp
similarity index 100%
rename from src/boards/Main/TMRepository/TMRepository.cpp
rename to src/boards/Main_old/TMRepository/TMRepository.cpp
diff --git a/src/boards/Main/TMRepository/TMRepository.h b/src/boards/Main_old/TMRepository/TMRepository.h
similarity index 100%
rename from src/boards/Main/TMRepository/TMRepository.h
rename to src/boards/Main_old/TMRepository/TMRepository.h
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index f005365af313df44c300a8b482fbad0f30e62c06..980948471951d10580a821b4354ef38765f4daef 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -20,299 +20,47 @@
  * THE SOFTWARE.
  */
 
-#include <Main/Actuators/Actuators.h>
-#include <Main/AltitudeTrigger/AltitudeTrigger.h>
-#include <Main/BoardScheduler.h>
 #include <Main/Buses.h>
-#include <Main/CanHandler/CanHandler.h>
-#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
-#include <Main/PinHandler/PinHandler.h>
-#include <Main/Radio/Radio.h>
 #include <Main/Sensors/Sensors.h>
-#include <Main/StateMachines/ABKController/ABKController.h>
-#include <Main/StateMachines/ADAController/ADAController.h>
-#include <Main/StateMachines/Deployment/Deployment.h>
-#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Main/StateMachines/MEAController/MEAController.h>
-#include <Main/StateMachines/NASController/NASController.h>
-#include <Main/TMRepository/TMRepository.h>
-#include <common/Events.h>
-#include <common/Topics.h>
-#include <diagnostic/CpuMeter/CpuMeter.h>
-#include <diagnostic/PrintLogger.h>
-#include <diagnostic/StackLogger.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <events/EventBroker.h>
-#include <events/EventData.h>
-#include <events/utils/EventSniffer.h>
+#include <Main/Radio/Radio.h>
+#include <drivers/timer/PWM.h>
+#include <miosix.h>
 
 #include <utils/ModuleManager/ModuleManager.hpp>
 
+using namespace miosix;
 using namespace Boardcore;
 using namespace Main;
-using namespace Common;
 
 int main()
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-
-    // Overall status, if at some point it becomes false, there is a problem
-    // somewhere
-    bool initResult    = true;
-    PrintLogger logger = Logging::getLogger("main");
-
-    // Create modules
-    BoardScheduler* scheduler = new BoardScheduler();
-    Buses* buses              = new Buses();
-    Sensors* sensors =
-        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
-    NASController* nas =
-        new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    ADAController* ada =
-        new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    TMRepository* tmRepo = new TMRepository();
-    CanHandler* canHandler =
-        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
-    FlightModeManager* fmm = new FlightModeManager();
-    Actuators* actuators =
-        new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY));
-    Deployment* dpl        = new Deployment();
-    PinHandler* pinHandler = new PinHandler();
-    AltitudeTrigger* altitudeTrigger =
-        new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    MEAController* mea =
-        new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    ABKController* abk =
-        new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX));
-    FlightStatsRecorder* recorder =
-        new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY));
-
-    // Insert modules
-    if (!modules.insert<BoardScheduler>(scheduler))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Board Scheduler module");
-    }
-
-    if (!modules.insert<Buses>(buses))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Buses module");
-    }
-
-    if (!modules.insert<Sensors>(sensors))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Sensor module");
-    }
-
-    if (!modules.insert<Actuators>(actuators))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Actuators module");
-    }
-
-    if (!modules.insert<Deployment>(dpl))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the DPL module");
-    }
-
-    if (!modules.insert<NASController>(nas))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the NAS module");
-    }
-
-    if (!modules.insert<ADAController>(ada))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the ADA module");
-    }
-
-    if (!modules.insert<MEAController>(mea))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the MEA module");
-    }
-
-    if (!modules.insert<ABKController>(abk))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the ABK controller module");
-    }
-
-    if (!modules.insert<Radio>(radio))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Radio module");
-    }
-
-    if (!modules.insert<FlightModeManager>(fmm))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the FMM module");
-    }
-
-    if (!modules.insert<TMRepository>(tmRepo))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the TMRepository module");
-    }
-
-    if (!modules.insert<CanHandler>(canHandler))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the CanHandler module");
-    }
-
-    if (!modules.insert<PinHandler>(pinHandler))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the PinHandler module");
-    }
-
-    if (!modules.insert<AltitudeTrigger>(altitudeTrigger))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Altitude Trigger module");
-    }
-
-    if (!modules.insert<FlightStatsRecorder>(recorder))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error inserting the Flight Stats Recorder module");
-    }
-
-    // Start modules
-    if (!Logger::getInstance().start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Logger module");
-    }
-
-    if (!EventBroker::getInstance().start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the EventBroker module");
-    }
-
-    if (!modules.get<BoardScheduler>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Board Scheduler module");
-    }
-
-    if (!modules.get<Actuators>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Actuators module");
-    }
-
-    if (!modules.get<Deployment>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Deployment module");
-    }
-
-    if (!modules.get<Sensors>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Sensors module");
-    }
-
-    if (!modules.get<NASController>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the NAS module");
-    }
-
-    if (!modules.get<ADAController>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the ADA module");
-    }
-
-    if (!modules.get<MEAController>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the MEA module");
-    }
-
-    if (!modules.get<ABKController>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the ABK controller module");
-    }
+    ModuleManager &modules = ModuleManager::getInstance();
 
-    if (!modules.get<FlightModeManager>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the FMM module");
-    }
+    TaskScheduler scheduler(2);
 
-    if (!modules.get<Radio>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Radio module");
-    }
+    Buses *buses = new Buses();
+    Sensors *sensors = new Sensors(scheduler);
+    Radio *radio = new Radio();
 
-    if (!modules.get<CanHandler>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the CanHandler module");
-    }
+    if(!modules.insert<Buses>(buses)) {
 
-    if (!modules.get<PinHandler>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the PinHandler module");
     }
 
-    if (!modules.get<AltitudeTrigger>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Altitude Trigger module");
-    }
+    if(!modules.insert<Sensors>(sensors)) {
 
-    if (!modules.get<FlightStatsRecorder>()->start())
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error starting the Flight Stats Recorder module");
     }
 
-    // Log all the events
-    EventSniffer sniffer(
-        EventBroker::getInstance(), TOPICS_LIST,
-        [](uint8_t event, uint8_t topic)
-        {
-            EventData ev{TimestampTimer::getTimestamp(), event, topic};
-            Logger::getInstance().log(ev);
-        });
-
-    // Check the init result and launch an event
-    if (initResult)
-    {
-        // Post OK
-        EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+    sensors->start();
+    radio->start();
+    scheduler.start();
 
-        // Set the LED status
-        miosix::led1On();
-    }
-    else
-    {
-        EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
-        LOG_ERR(logger, "Failed to initialize");
+    for(auto &info : sensors->getSensorInfo()) {
+        printf("Sensor: %s %d\n", info.id.c_str(), info.isInitialized);
     }
 
-    // Periodic statistics
     while (true)
     {
         Thread::sleep(1000);
-        Logger::getInstance().log(CpuMeter::getCpuStats());
-        CpuMeter::resetCpuStats();
-        StackLogger::getInstance().log();
     }
 
     return 0;
diff --git a/src/entrypoints/Main_old/main-entry.cpp b/src/entrypoints/Main_old/main-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f005365af313df44c300a8b482fbad0f30e62c06
--- /dev/null
+++ b/src/entrypoints/Main_old/main-entry.cpp
@@ -0,0 +1,319 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: 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 <Main/Actuators/Actuators.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Buses.h>
+#include <Main/CanHandler/CanHandler.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ABKController/ABKController.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/Deployment/Deployment.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Main;
+using namespace Common;
+
+int main()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Overall status, if at some point it becomes false, there is a problem
+    // somewhere
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+
+    // Create modules
+    BoardScheduler* scheduler = new BoardScheduler();
+    Buses* buses              = new Buses();
+    Sensors* sensors =
+        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+    NASController* nas =
+        new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    ADAController* ada =
+        new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    TMRepository* tmRepo = new TMRepository();
+    CanHandler* canHandler =
+        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    FlightModeManager* fmm = new FlightModeManager();
+    Actuators* actuators =
+        new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY));
+    Deployment* dpl        = new Deployment();
+    PinHandler* pinHandler = new PinHandler();
+    AltitudeTrigger* altitudeTrigger =
+        new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    MEAController* mea =
+        new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    ABKController* abk =
+        new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    FlightStatsRecorder* recorder =
+        new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY));
+
+    // Insert modules
+    if (!modules.insert<BoardScheduler>(scheduler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Board Scheduler module");
+    }
+
+    if (!modules.insert<Buses>(buses))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Buses module");
+    }
+
+    if (!modules.insert<Sensors>(sensors))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Sensor module");
+    }
+
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Actuators module");
+    }
+
+    if (!modules.insert<Deployment>(dpl))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the DPL module");
+    }
+
+    if (!modules.insert<NASController>(nas))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the NAS module");
+    }
+
+    if (!modules.insert<ADAController>(ada))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the ADA module");
+    }
+
+    if (!modules.insert<MEAController>(mea))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the MEA module");
+    }
+
+    if (!modules.insert<ABKController>(abk))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the ABK controller module");
+    }
+
+    if (!modules.insert<Radio>(radio))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Radio module");
+    }
+
+    if (!modules.insert<FlightModeManager>(fmm))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the FMM module");
+    }
+
+    if (!modules.insert<TMRepository>(tmRepo))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the TMRepository module");
+    }
+
+    if (!modules.insert<CanHandler>(canHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the CanHandler module");
+    }
+
+    if (!modules.insert<PinHandler>(pinHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the PinHandler module");
+    }
+
+    if (!modules.insert<AltitudeTrigger>(altitudeTrigger))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Altitude Trigger module");
+    }
+
+    if (!modules.insert<FlightStatsRecorder>(recorder))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Flight Stats Recorder module");
+    }
+
+    // Start modules
+    if (!Logger::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Logger module");
+    }
+
+    if (!EventBroker::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the EventBroker module");
+    }
+
+    if (!modules.get<BoardScheduler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Board Scheduler module");
+    }
+
+    if (!modules.get<Actuators>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Actuators module");
+    }
+
+    if (!modules.get<Deployment>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Deployment module");
+    }
+
+    if (!modules.get<Sensors>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Sensors module");
+    }
+
+    if (!modules.get<NASController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the NAS module");
+    }
+
+    if (!modules.get<ADAController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ADA module");
+    }
+
+    if (!modules.get<MEAController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the MEA module");
+    }
+
+    if (!modules.get<ABKController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ABK controller module");
+    }
+
+    if (!modules.get<FlightModeManager>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the FMM module");
+    }
+
+    if (!modules.get<Radio>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Radio module");
+    }
+
+    if (!modules.get<CanHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the CanHandler module");
+    }
+
+    if (!modules.get<PinHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the PinHandler module");
+    }
+
+    if (!modules.get<AltitudeTrigger>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Altitude Trigger module");
+    }
+
+    if (!modules.get<FlightStatsRecorder>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Flight Stats Recorder module");
+    }
+
+    // Log all the events
+    EventSniffer sniffer(
+        EventBroker::getInstance(), TOPICS_LIST,
+        [](uint8_t event, uint8_t topic)
+        {
+            EventData ev{TimestampTimer::getTimestamp(), event, topic};
+            Logger::getInstance().log(ev);
+        });
+
+    // Check the init result and launch an event
+    if (initResult)
+    {
+        // Post OK
+        EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+
+        // Set the LED status
+        miosix::led1On();
+    }
+    else
+    {
+        EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+        LOG_ERR(logger, "Failed to initialize");
+    }
+
+    // Periodic statistics
+    while (true)
+    {
+        Thread::sleep(1000);
+        Logger::getInstance().log(CpuMeter::getCpuStats());
+        CpuMeter::resetCpuStats();
+        StackLogger::getInstance().log();
+    }
+
+    return 0;
+}
\ No newline at end of file