diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index 8f89928081b6b4dfc5c2594e8cc9c52dab05b779..1b9b1d2dee0f4561ca5377f013e3ef9620fa755b 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -52,6 +52,8 @@ public:
         };
     };
 
+    Boardcore::TaskScheduler& sensors() { return high; }
+
     static Priority::PriorityLevel flightModeManagerPriority()
     {
         return Priority::MEDIUM;
diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7b8a5e536088dee32725c5e68f39eee7ba4e2cf
--- /dev/null
+++ b/src/Parafoil/Configs/SensorsConfig.h
@@ -0,0 +1,129 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08Defs.h>
+#include <sensors/BMX160/BMX160Config.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS3MDL/LIS3MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <units/Frequency.h>
+
+#include <chrono>
+
+namespace Parafoil
+{
+namespace Config
+{
+namespace Sensors
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace std::chrono_literals;
+
+namespace BMX160
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+
+constexpr auto CALIBRATION_DURATION = 2000ms;
+constexpr auto ACC_FSR      = Boardcore::BMX160Config::AccelerometerRange::G_16;
+constexpr auto GYRO_FSR     = Boardcore::BMX160Config::GyroscopeRange::DEG_1000;
+constexpr auto ACC_GYRO_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_200;
+constexpr auto MAG_ODR      = Boardcore::BMX160Config::OutputDataRate::HZ_100;
+
+constexpr auto AXIS_ROTATION = {Boardcore::Direction::NEGATIVE_Y,
+                                Boardcore::Direction::NEGATIVE_Z};
+
+constexpr auto TEMP_DIVIDER   = 1000;
+constexpr auto FIFO_WATERMARK = 40;
+
+constexpr auto FIFO_HEADER_SIZE = 1;
+constexpr auto ACC_DATA_SIZE    = 6;
+constexpr auto GYRO_DATA_SIZE   = 6;
+constexpr auto MAG_DATA_SIZE    = 8;
+}  // namespace BMX160
+
+namespace H3LIS331DL
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto ODR = Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
+constexpr auto BDU =
+    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+constexpr auto FSR = Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+}  // namespace H3LIS331DL
+
+namespace LPS22DF
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 50_hz;
+constexpr auto AVG           = Boardcore::LPS22DF::AVG_4;
+constexpr auto ODR           = Boardcore::LPS22DF::ODR_100;
+}  // namespace LPS22DF
+
+namespace UBXGPS
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 10_hz;
+}  // namespace UBXGPS
+
+namespace ADS131M08
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto OVERSAMPLING_RATIO =
+    Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+constexpr bool GLOBAL_CHOP_MODE = true;
+}  // namespace ADS131M08
+
+namespace LIS3MDL
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto ODR           = Boardcore::LIS3MDL::ODR_80_HZ;
+constexpr auto FSR           = Boardcore::LIS3MDL::FS_4_GAUSS;
+}  // namespace LIS3MDL
+
+namespace InternalADC
+{
+constexpr auto ENABLED       = true;
+constexpr auto SAMPLING_RATE = 100_hz;
+constexpr auto VBAT_CH       = Boardcore::InternalADC::Channel::CH5;
+constexpr auto VBAT_COEFF    = (150 + 40.2) / 40.2;
+}  // namespace InternalADC
+
+namespace MagCalibration
+{
+constexpr auto FILE_ENABLED     = true;
+constexpr auto SAMPLING_RATE    = 10_hz;
+constexpr auto CALIBRATION_PATH = "/sd/bmx160_magnetometer_calibration.csv";
+}  // namespace MagCalibration
+
+}  // namespace Sensors
+
+}  // namespace Config
+
+}  // namespace Parafoil
\ No newline at end of file
diff --git a/src/Parafoil/Sensors/SensorData.h b/src/Parafoil/Sensors/SensorData.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf7a88d15f2c3738e030ce07887597c7e903e953
--- /dev/null
+++ b/src/Parafoil/Sensors/SensorData.h
@@ -0,0 +1,53 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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/SensorData.h>
+
+namespace Parafoil
+{
+
+struct SensorCalibrationData
+{
+    uint64_t timestamp = 0;
+    float magBiasX     = 0.0f;
+    float magBiasY     = 0.0f;
+    float magBiasZ     = 0.0f;
+    float magScaleX    = 0.0f;
+    float magScaleY    = 0.0f;
+    float magScaleZ    = 0.0f;
+
+    static std::string header()
+    {
+        return "timestamp,magBiasX,magBiasY,magBiasZ,magScaleX,magScaleY,"
+               "magScaleZ\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << magBiasX << "," << magBiasY << "," << magBiasZ
+           << "," << magScaleX << "," << magScaleY << "," << magScaleZ << "\n";
+    }
+};
+
+}  // namespace Parafoil
\ No newline at end of file
diff --git a/src/Parafoil/Sensors/Sensors.cpp b/src/Parafoil/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a52727c36fd0a43d7cd5c6984e236e168ec7a984
--- /dev/null
+++ b/src/Parafoil/Sensors/Sensors.cpp
@@ -0,0 +1,511 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * 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 <Parafoil/BoardScheduler.h>
+#include <Parafoil/Buses.h>
+
+#include <chrono>
+
+using namespace Boardcore;
+namespace config = Parafoil::Config::Sensors;
+namespace hwmap  = miosix::sensors;
+
+namespace Parafoil
+{
+
+bool Sensors::start()
+{
+    if (config::BMX160::ENABLED)
+    {
+        bmx160Init();
+        bmx160WithCorrectionInit();
+    }
+
+    if (config::LIS3MDL::ENABLED)
+    {
+        lis3mdlInit();
+    }
+
+    if (config::H3LIS331DL::ENABLED)
+    {
+        h3lisInit();
+    }
+
+    if (config::LPS22DF::ENABLED)
+    {
+        lps22Init();
+    }
+
+    if (config::UBXGPS::ENABLED)
+    {
+        ubxGpsInit();
+    }
+
+    if (config::ADS131M08::ENABLED)
+    {
+        ads131Init();
+    }
+
+    if (config::InternalADC::ENABLED)
+    {
+        internalADCInit();
+    }
+
+    if (!postSensorCreationHook())
+    {
+        LOG_ERR(logger, "Sensors post-creation hook failed");
+        return false;
+    }
+
+    if (!sensorManagerInit())
+    {
+        LOG_ERR(logger, "Failed to initialize sensor manager");
+        return false;
+    }
+
+    auto& scheduler = getModule<BoardScheduler>()->sensors();
+
+    magCalibrationTaskId = scheduler.addTask(
+        [this]()
+        {
+            auto mag = getLIS3MDLLastSample();
+
+            miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+            magCalibrator.feed(mag);
+        },
+        config::MagCalibration::SAMPLING_RATE);
+
+    if (magCalibrationTaskId == 0)
+    {
+        LOG_ERR(logger, "Failed to add magnetometer calibration task");
+        return false;
+    }
+
+    // Immediately disable the task, so that is enabled only when needed
+    // from FlightModeManager. It is calibrated during the pre-flight
+    // initialization phase.
+    scheduler.disableTask(magCalibrationTaskId);
+
+    started = true;
+    return true;
+}
+
+bool Sensors::isStarted() { return started; }
+
+void Sensors::calibrate()
+{
+    bmx160WithCorrection->startCalibration();
+
+    miosix::Thread::sleep(
+        std::chrono::milliseconds{config::BMX160::CALIBRATION_DURATION}
+            .count());
+
+    bmx160WithCorrection->stopCalibration();
+}
+
+SensorCalibrationData Sensors::getCalibrationData()
+{
+    miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+
+    return SensorCalibrationData{
+        .timestamp = TimestampTimer::getTimestamp(),
+        .magBiasX  = magCalibration.getb().x(),
+        .magBiasY  = magCalibration.getb().y(),
+        .magBiasZ  = magCalibration.getb().z(),
+        .magScaleX = magCalibration.getA().x(),
+        .magScaleY = magCalibration.getA().y(),
+        .magScaleZ = magCalibration.getA().z(),
+    };
+}
+
+void Sensors::resetMagCalibrator()
+{
+    miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+    magCalibrator = SoftAndHardIronCalibration{};
+}
+
+void Sensors::enableMagCalibrator()
+{
+    getModule<BoardScheduler>()->sensors().enableTask(magCalibrationTaskId);
+}
+
+void Sensors::disableMagCalibrator()
+{
+    getModule<BoardScheduler>()->sensors().disableTask(magCalibrationTaskId);
+}
+
+bool Sensors::saveMagCalibration()
+{
+    miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex};
+
+    SixParametersCorrector calibration = magCalibrator.computeResult();
+
+    // Check if the calibration is valid
+    if (!std::isnan(calibration.getb()[0]) &&
+        !std::isnan(calibration.getb()[1]) &&
+        !std::isnan(calibration.getb()[2]) &&
+        !std::isnan(calibration.getA()[0]) &&
+        !std::isnan(calibration.getA()[1]) &&
+        !std::isnan(calibration.getA()[2]))
+    {
+        // Its valid, save it and apply it
+        magCalibration = calibration;
+        return magCalibration.toFile(config::MagCalibration::CALIBRATION_PATH);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+BMX160Data Sensors::getBMX160LastSample()
+{
+    return bmx160 ? bmx160->getLastSample() : BMX160Data{};
+}
+
+BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample()
+{
+    return bmx160WithCorrection ? bmx160WithCorrection->getLastSample()
+                                : BMX160WithCorrectionData{};
+}
+
+LIS3MDLData Sensors::getLIS3MDLLastSample()
+{
+    return lis3mdl ? lis3mdl->getLastSample() : LIS3MDLData{};
+}
+
+H3LIS331DLData Sensors::getH3LISLastSample()
+{
+    return h3lis331dl ? h3lis331dl->getLastSample() : H3LIS331DLData{};
+}
+
+LPS22DFData Sensors::getLPS22DFLastSample()
+{
+    return lps22df ? lps22df->getLastSample() : LPS22DFData{};
+}
+
+UBXGPSData Sensors::getUBXGPSLastSample()
+{
+    return ubxgps ? ubxgps->getLastSample() : UBXGPSData{};
+}
+
+ADS131M08Data Sensors::getADS131LastSample()
+{
+    return ads131m08 ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+InternalADCData Sensors::getInternalADCLastSample()
+{
+    return internalAdc ? internalAdc->getLastSample() : InternalADCData{};
+}
+
+BatteryVoltageSensorData Sensors::getBatteryVoltage()
+{
+    auto sample = getInternalADCLastSample();
+
+    BatteryVoltageSensorData data;
+    data.voltageTimestamp = sample.timestamp;
+    data.channelId        = static_cast<uint8_t>(config::InternalADC::VBAT_CH);
+    data.voltage          = sample.voltage[config::InternalADC::VBAT_CH];
+    data.batVoltage       = sample.voltage[config::InternalADC::VBAT_CH] *
+                      config::InternalADC::VBAT_COEFF;
+
+    return data;
+}
+
+std::vector<SensorInfo> Sensors::getSensorInfo()
+{
+    if (manager)
+    {
+        std::vector<SensorInfo> infos{};
+
+#define PUSH_SENSOR_INFO(instance, name)                         \
+    if (instance)                                                \
+        infos.push_back(manager->getSensorInfo(instance.get())); \
+    else                                                         \
+        infos.push_back(                                         \
+            SensorInfo { #name, config::name::SAMPLING_RATE, nullptr, false })
+
+        PUSH_SENSOR_INFO(bmx160, BMX160);
+        PUSH_SENSOR_INFO(bmx160WithCorrection, BMX160);
+        PUSH_SENSOR_INFO(h3lis331dl, H3LIS331DL);
+        PUSH_SENSOR_INFO(lis3mdl, LIS3MDL);
+        PUSH_SENSOR_INFO(lps22df, LPS22DF);
+        PUSH_SENSOR_INFO(ubxgps, UBXGPS);
+        PUSH_SENSOR_INFO(ads131m08, ADS131M08);
+        PUSH_SENSOR_INFO(internalAdc, InternalADC);
+
+#undef PUSH_SENSOR_INFO
+
+        return infos;
+    }
+    else
+    {
+        return {};
+    }
+}
+
+void Sensors::bmx160Init()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_4;
+
+    BMX160Config config;
+    config.fifoMode      = BMX160Config::FifoMode::DISABLED;
+    config.fifoWatermark = config::BMX160::FIFO_WATERMARK;
+    config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    config.temperatureDivider = config::BMX160::TEMP_DIVIDER;
+
+    config.accelerometerRange = config::BMX160::ACC_FSR;
+    config.gyroscopeRange     = config::BMX160::GYRO_FSR;
+
+    config.accelerometerDataRate = config::BMX160::ACC_GYRO_ODR;
+    config.gyroscopeDataRate     = config::BMX160::ACC_GYRO_ODR;
+    config.magnetometerRate      = config::BMX160::MAG_ODR;
+
+    config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    bmx160 = std::make_unique<BMX160>(getModule<Buses>()->spi1,
+                                      hwmap::bmx160::cs::getPin(), spiConfig,
+                                      config);
+
+    LOG_INFO(logger, "BMX160 initialized!");
+}
+
+void Sensors::bmx160WithCorrectionInit()
+{
+    bmx160WithCorrection = std::make_unique<BMX160WithCorrection>(
+        bmx160.get(), config::BMX160::AXIS_ROTATION);
+
+    LOG_INFO(logger, "BMX160WithCorrection initialized!");
+}
+
+void Sensors::h3lisInit()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
+
+    h3lis331dl = std::make_unique<H3LIS331DL>(
+        getModule<Buses>()->spi1, hwmap::h3lis331dl::cs::getPin(), spiConfig,
+        config::H3LIS331DL::ODR, config::H3LIS331DL::BDU,
+        config::H3LIS331DL::FSR);
+
+    LOG_INFO(logger, "H3LIS331DL initialized!");
+}
+
+void Sensors::lis3mdlInit()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+
+    LIS3MDL::Config config;
+    config.odr                = config::LIS3MDL::ODR;
+    config.scale              = config::LIS3MDL::FSR;
+    config.temperatureDivider = 1;
+
+    lis3mdl = std::make_unique<LIS3MDL>(getModule<Buses>()->spi1,
+                                        hwmap::lis3mdl::cs::getPin(), spiConfig,
+                                        config);
+
+    LOG_INFO(logger, "LIS3MDL initialized!");
+}
+
+void Sensors::lps22Init()
+{
+    auto spiConfig         = LPS22DF::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_16;
+
+    LPS22DF::Config config;
+    config.avg = config::LPS22DF::AVG;
+    config.odr = config::LPS22DF::ODR;
+
+    lps22df = std::make_unique<LPS22DF>(getModule<Buses>()->spi1,
+                                        hwmap::lps22df::cs::getPin(), spiConfig,
+                                        config);
+
+    LOG_INFO(logger, "LPS22DF initialized!");
+}
+
+void Sensors::ubxGpsInit()
+{
+    auto spiConfig         = UBXGPSSpi::getDefaultSPIConfig();
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
+
+    ubxgps = std::make_unique<UBXGPSSpi>(getModule<Buses>()->spi1,
+                                         hwmap::ubxgps::cs::getPin(), spiConfig,
+                                         config::UBXGPS::SAMPLING_RATE);
+
+    LOG_INFO(logger, "UBXGPS initialized!");
+}
+
+void Sensors::ads131Init()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+
+    ADS131M08::Config config;
+    config.oversamplingRatio     = config::ADS131M08::OVERSAMPLING_RATIO;
+    config.globalChopModeEnabled = config::ADS131M08::GLOBAL_CHOP_MODE;
+
+    ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->spi1,
+                                            hwmap::ads131m08::cs::getPin(),
+                                            spiConfig, config);
+
+    LOG_INFO(logger, "ADS131M08 initialized!");
+}
+
+void Sensors::internalADCInit()
+{
+    internalAdc = std::make_unique<InternalADC>(ADC3);
+    internalAdc->enableChannel(config::InternalADC::VBAT_CH);
+    internalAdc->enableTemperature();
+    internalAdc->enableVbat();
+
+    LOG_INFO(logger, "InternalADC initialized!");
+}
+
+void Sensors::bmx160Callback()
+{
+    auto fifoSize = bmx160->getLastFifoSize();
+    auto& fifo    = bmx160->getLastFifo();
+
+    Logger::getInstance().log(bmx160->getTemperature());
+
+    for (auto i = 0; i < fifoSize; i++)
+    {
+        Logger::getInstance().log(fifo.at(i));
+    }
+
+    Logger::getInstance().log(bmx160->getFifoStats());
+}
+
+void Sensors::bmx160WithCorrectionCallback()
+{
+    BMX160WithCorrectionData lastSample = bmx160WithCorrection->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::h3lisCallback()
+{
+    H3LIS331DLData lastSample = h3lis331dl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::lis3mdlCallback()
+{
+    LIS3MDLData lastSample = lis3mdl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::lps22Callback()
+{
+    LPS22DFData lastSample = lps22df->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::ubxGpsCallback()
+{
+    UBXGPSData lastSample = ubxgps->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::ads131Callback()
+{
+    ADS131M08Data lastSample = ads131m08->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+void Sensors::internalADCCallback()
+{
+    InternalADCData lastSample = internalAdc->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+bool Sensors::sensorManagerInit()
+{
+    SensorManager::SensorMap_t map;
+
+    if (bmx160)
+    {
+        SensorInfo info{"BMX160", config::BMX160::SAMPLING_RATE,
+                        [this]() { bmx160Callback(); }};
+        map.emplace(bmx160.get(), info);
+    }
+
+    if (bmx160WithCorrection)
+    {
+        SensorInfo info{"BMX160WithCorrection", config::BMX160::SAMPLING_RATE,
+                        [this]() { bmx160WithCorrectionCallback(); }};
+        map.emplace(bmx160WithCorrection.get(), info);
+    }
+
+    if (h3lis331dl)
+    {
+        SensorInfo info{"H3LIS331DL", config::H3LIS331DL::SAMPLING_RATE,
+                        [this]() { h3lisCallback(); }};
+        map.emplace(h3lis331dl.get(), info);
+    }
+
+    if (lis3mdl)
+    {
+        SensorInfo info{"LIS3MDL", config::LIS3MDL::SAMPLING_RATE,
+                        [this]() { lis3mdlCallback(); }};
+        map.emplace(lis3mdl.get(), info);
+    }
+
+    if (lps22df)
+    {
+        SensorInfo info{"LPS22DF", config::LPS22DF::SAMPLING_RATE,
+                        [this]() { lps22Callback(); }};
+        map.emplace(lps22df.get(), info);
+    }
+
+    if (ubxgps)
+    {
+        SensorInfo info{"UBXGPS", config::UBXGPS::SAMPLING_RATE,
+                        [this]() { ubxGpsCallback(); }};
+        map.emplace(ubxgps.get(), info);
+    }
+
+    if (ads131m08)
+    {
+        SensorInfo info{"ADS131M08", config::ADS131M08::SAMPLING_RATE,
+                        [this]() { ads131Callback(); }};
+        map.emplace(ads131m08.get(), info);
+    }
+
+    if (internalAdc)
+    {
+        SensorInfo info{"InternalADC", config::InternalADC::SAMPLING_RATE,
+                        [this]() { internalADCCallback(); }};
+        map.emplace(internalAdc.get(), info);
+    }
+
+    auto& scheduler = getModule<BoardScheduler>()->sensors();
+    manager         = std::make_unique<SensorManager>(map, &scheduler);
+    return manager->start();
+}
+
+}  // namespace Parafoil
\ No newline at end of file
diff --git a/src/Parafoil/Sensors/Sensors.h b/src/Parafoil/Sensors/Sensors.h
index 39c70a6d8ed71ad5a42f6925a577124b65811db2..46f40b8cbdb170c5f200c98d580dcbe95df4336a 100644
--- a/src/Parafoil/Sensors/Sensors.h
+++ b/src/Parafoil/Sensors/Sensors.h
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <Parafoil/Configs/SensorsConfig.h>
 #include <drivers/adc/InternalADC.h>
 #include <sensors/ADS131M08/ADS131M08.h>
 #include <sensors/BMX160/BMX160.h>
@@ -32,8 +33,11 @@
 #include <sensors/SensorManager.h>
 #include <sensors/UBXGPS/UBXGPSSpi.h>
 #include <sensors/analog/BatteryVoltageSensor.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
 #include <utils/DependencyManager/DependencyManager.h>
 
+#include "SensorData.h"
+
 namespace Parafoil
 {
 class BoardScheduler;
@@ -60,22 +64,45 @@ public:
     void calibrate();
 
     /**
-     * @brief Takes the result of the live magnetometer calibration and applies
-     * it to the current calibration + writes it in the csv file
+     * @brief Returns the current sensors calibration parameters.
      *
-     * @return true if the write was successful
+     * @internal Ensure mutexes are unlocked before calling this function.
+     */
+    SensorCalibrationData getCalibrationData();
+
+    /**
+     * @brief Resets the magnetometer calibration.
+     */
+    void resetMagCalibrator();
+
+    /**
+     * @brief Enables the magnetometer calibration task.
+     */
+    void enableMagCalibrator();
+
+    /**
+     * @brief Disables the magnetometer calibration task.
      */
-    bool writeMagCalibration();
+    void disableMagCalibrator();
+
+    /**
+     * @brief Saves the current magnetometer calibration to file, overwriting
+     * the previous one, and sets it as the active calibration.
+     *
+     * @return Whether the calibration was saved successfully.
+     */
+    bool saveMagCalibration();
 
     Boardcore::BMX160Data getBMX160LastSample();
     Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample();
     Boardcore::H3LIS331DLData getH3LISLastSample();
     Boardcore::LIS3MDLData getLIS3MDLLastSample();
-    Boardcore::LPS22DFData getLPS22LastSample();
-    Boardcore::UBXGPSData getUbxGpsLastSample();
+    Boardcore::LPS22DFData getLPS22DFLastSample();
+    Boardcore::UBXGPSData getUBXGPSLastSample();
     Boardcore::ADS131M08Data getADS131LastSample();
     Boardcore::InternalADCData getInternalADCLastSample();
-    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+
+    Boardcore::BatteryVoltageSensorData getBatteryVoltage();
 
     /**
      * @brief Returns information about all sensors managed by this class
@@ -97,7 +124,6 @@ protected:
     virtual bool postSensorCreationHook() { return true; }
 
     std::unique_ptr<Boardcore::BMX160> bmx160;
-    std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection;
     std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
     std::unique_ptr<Boardcore::LIS3MDL> lis3mdl;
     std::unique_ptr<Boardcore::LPS22DF> lps22df;
@@ -105,6 +131,10 @@ protected:
     std::unique_ptr<Boardcore::ADS131M08> ads131m08;
     std::unique_ptr<Boardcore::InternalADC> internalAdc;
 
+    std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection;
+
+    std::unique_ptr<Boardcore::SensorManager> manager;
+
 private:
     /**
      * Sensors initialization and callback functions.
@@ -134,10 +164,17 @@ private:
     void internalADCInit();
     void internalADCCallback();
 
-    void batteryVoltageInit();
-    void batteryVoltageCallback();
-
     bool sensorManagerInit();
+
+    // Live calibration of the magnetomer
+    miosix::FastMutex magCalibrationMutex;
+    Boardcore::SoftAndHardIronCalibration magCalibrator;
+    Boardcore::SixParametersCorrector magCalibration;
+    size_t magCalibrationTaskId = 0;
+
+    std::atomic<bool> started{false};
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
 };
 
 }  // namespace Parafoil
\ No newline at end of file
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index 8c07feaae339f268793159054e792d909dae5144..a38d1413b9d4f44a454c0b8fa66624fc610abf64 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -39,8 +39,7 @@ using namespace Parafoil;
 
 int main()
 {
-    std::cout << "Parafoil Entrypoint "
-              << "(" << BUILD_TYPE << ")"
+    std::cout << "Parafoil Entrypoint " << "(" << BUILD_TYPE << ")"
               << " by Skyward Experimental Rocketry" << std::endl;
 
     auto logger = Logging::getLogger("Mockup");
diff --git a/src/Payload/payload-entry.cpp b/src/Payload/payload-entry.cpp
index a2160a7abf826f046417bfad198ea0429229a741..81fd9cb4d77cea0ef6c7391d230fbde98d60228c 100644
--- a/src/Payload/payload-entry.cpp
+++ b/src/Payload/payload-entry.cpp
@@ -106,9 +106,8 @@ using namespace Common;
 int main()
 {
     miosix::ledOff();
-    std::cout << "Payload " << FLAVOR << " Entrypoint "
-              << "(" << BUILD_TYPE << ")"
-              << " by Skyward Experimental Rocketry" << std::endl;
+    std::cout << "Payload " << FLAVOR << " Entrypoint " << "(" << BUILD_TYPE
+              << ")" << " by Skyward Experimental Rocketry" << std::endl;
 
     // Unused but needed to set the log level properly
     auto logger = Logging::getLogger("Payload");