diff --git a/CMakeLists.txt b/CMakeLists.txt
index 72dfec3d779256845c691e855da0396714f59ce2..83d05228c5f8d06d1276fc95447bd2514fd84375 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -117,6 +117,12 @@ sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
 add_executable(test-trace-logger src/tests/test-trace-logger.cpp)
 sbs_target(test-trace-logger stm32f429zi_stm32f4discovery)
 
+add_executable(test-hil 
+    src/tests/hil/test-hil.cpp 
+    src/tests/hil/Sensors/Sensors.cpp 
+)
+sbs_target(test-hil stm32f767zi_death_stack_v4)
+
 #-----------------------------------------------------------------------------#
 #                                Tests - Catch                                #
 #-----------------------------------------------------------------------------#
diff --git a/src/shared/hil/HIL.h b/src/shared/hil/HIL.h
new file mode 100644
index 0000000000000000000000000000000000000000..bb11d0174836f3be5d461392075e64df25297d4a
--- /dev/null
+++ b/src/shared/hil/HIL.h
@@ -0,0 +1,154 @@
+/* Copyright (c) 2021-2024 Skyward Experimental Rocketry
+ * Authors: Luca Conterio, Emilio Corigliano
+ *
+ * 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 <Singleton.h>
+#include <diagnostic/SkywardStack.h>
+#include <utils/KernelTime.h>
+#include <utils/TimeUtils.h>
+
+#include "HILPhasesManager.h"
+#include "HILTransceiver.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Single interface to the hardware-in-the-loop framework.
+ */
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HIL : public Boardcore::ActiveObject
+{
+    using PhasesCallback =
+        typename HILPhasesManager<FlightPhases, SimulatorData,
+                                  ActuatorData>::PhasesCallback;
+
+public:
+    /**
+     * @brief Constructor of the HIL framework.
+     * @param hilTransceiver The pointer to the already built HILTransceiver.
+     * @param hilPhasesManager The pointer to the already built
+     * HILPhasesManager.
+     * @param updateActuatorData Function which returns the current ActuatorData
+     * situation.
+     * @param simulationPeriod Period of the simulation [ms].
+     */
+    HIL(HILTransceiver<FlightPhases, SimulatorData, ActuatorData>
+            *hilTransceiver,
+        HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>
+            *hilPhasesManager,
+        std::function<ActuatorData()> updateActuatorData, int simulationPeriod)
+        : Boardcore::ActiveObject(Boardcore::STACK_MIN_FOR_SKYWARD,
+                                  miosix::PRIORITY_MAX - 1),
+          hilTransceiver(hilTransceiver), hilPhasesManager(hilPhasesManager),
+          updateActuatorData(updateActuatorData),
+          simulationPeriod(simulationPeriod)
+    {
+    }
+
+    /**
+     * @brief Start the needed hardware-in-the-loop components.
+     */
+    [[nodiscard]] bool start() override
+    {
+        bool initOk = true;
+
+        if (!hilTransceiver->start())
+        {
+            LOG_ERR(logger, "hilTransceiver started with errors");
+            initOk = false;
+        }
+
+        if (!hilPhasesManager->start())
+        {
+            LOG_ERR(logger, "hilPhasesManager started with errors");
+            initOk = false;
+        }
+
+        if (!Boardcore::ActiveObject::start())
+        {
+            LOG_ERR(logger, "hil started with errors");
+            initOk = false;
+        }
+
+        return initOk;
+    }
+
+    void stop()
+    {
+        hilTransceiver->stop();
+        hilPhasesManager->stop();
+        Boardcore::ActiveObject::stop();
+        LOG_INFO(logger, "HIL framework stopped");
+    }
+
+    void waitStartSimulation()
+    {
+        LOG_INFO(logger, "Waiting for simulation to start...");
+        while (!hilPhasesManager->isSimulationRunning())
+        {
+            miosix::Thread::sleep(1);
+        }
+    }
+
+    void registerToFlightPhase(const FlightPhases &flag,
+                               const PhasesCallback &func)
+    {
+        hilPhasesManager->registerToFlightPhase(flag, func);
+    }
+
+    int getSimulationPeriod() const { return simulationPeriod; }
+
+    int64_t getTimestampSimulatorData() const
+    {
+        return hilTransceiver->getTimestampSimulatorData();
+    }
+
+    const SimulatorData *getSensorData() const
+    {
+        return hilTransceiver->getSensorData();
+    }
+
+protected:
+    HILTransceiver<FlightPhases, SimulatorData, ActuatorData> *hilTransceiver;
+    HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>
+        *hilPhasesManager;
+
+private:
+    void run() override
+    {
+        uint64_t ts = miosix::getTime();
+        while (!shouldStop())
+        {
+            ts += msToNs(simulationPeriod);
+            hilTransceiver->setActuatorData(updateActuatorData());
+            miosix::Thread::nanoSleepUntil(ts);
+        }
+    }
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("HIL");
+
+    std::function<ActuatorData()> updateActuatorData;
+    int simulationPeriod;  // Simulation period in milliseconds
+};
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/hil/HILPhasesManager.h b/src/shared/hil/HILPhasesManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..db00cb7d30936431f21a1f09e848579bc785e65f
--- /dev/null
+++ b/src/shared/hil/HILPhasesManager.h
@@ -0,0 +1,196 @@
+/* Copyright (c) 2021-2024 Skyward Experimental Rocketry
+ * Authors: Luca Conterio, Emilio Corigliano
+ *
+ * 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 <algorithms/AirBrakes/TrajectoryPoint.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/Event.h>
+#include <events/EventHandler.h>
+#include <miosix.h>
+#include <utils/Debug.h>
+
+#include <iostream>
+#include <map>
+
+namespace Boardcore
+{
+
+struct Outcomes
+{
+    uint64_t t = 0;
+    float z    = 0;
+    float vz   = 0;
+
+    Outcomes() : t(0), z(0), vz(0) {}
+    Outcomes(float z, float vz)
+        : t(TimestampTimer::getTimestamp()), z(z), vz(vz)
+    {
+    }
+
+    void print(uint64_t t_start) const
+    {
+        std::cout << "@time     : " << (double)(t - t_start) / 1000000
+                  << " [sec]\n";
+        std::cout << "@z : " << z << " [m]\n";
+        std::cout << "@vz : " << vz << " [m/s]\n\n";
+    }
+};
+
+class HILPhasesManagerBase : public EventHandler
+{
+public:
+    explicit HILPhasesManagerBase(
+        std::function<TimedTrajectoryPoint()> getCurrentPosition)
+        : EventHandler(), getCurrentPosition(getCurrentPosition)
+    {
+    }
+
+    bool isSimulationRunning() const { return (t_start != 0) && (t_stop == 0); }
+
+    virtual void simulationStarted()
+    {
+        t_start = TimestampTimer::getTimestamp();
+        LOG_INFO(logger, "SIMULATION STARTED!");
+    }
+
+    virtual void liftoff()
+    {
+        t_liftoff = TimestampTimer::getTimestamp();
+        LOG_INFO(logger, "LIFTOFF!");
+    }
+
+    virtual void simulationStopped()
+    {
+        t_stop = TimestampTimer::getTimestamp();
+        LOG_INFO(logger, "SIMULATION STOPPED!");
+    }
+
+protected:
+    uint64_t t_start   = 0;
+    uint64_t t_liftoff = 0;
+    uint64_t t_stop    = 0;
+    std::function<TimedTrajectoryPoint()> getCurrentPosition;
+    PrintLogger logger = Logging::getLogger("HILPhasesManager");
+};
+
+/**
+ * @brief Singleton object that manages all the phases of the simulation.
+ * After his instantiation we need to set the source of the current position in
+ * order to be able to save the outcomes for each event.
+ */
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HILPhasesManager : public HILPhasesManagerBase
+{
+public:
+    using PhasesCallback = std::function<void()>;
+
+    explicit HILPhasesManager(
+        std::function<TimedTrajectoryPoint()> getCurrentPosition)
+        : HILPhasesManagerBase(getCurrentPosition)
+    {
+    }
+
+    bool isFlagActive(const FlightPhases& flag) const
+    {
+        return flagsFlightPhases.at(flag);
+    }
+
+    void registerToFlightPhase(const FlightPhases& flag,
+                               const PhasesCallback& func)
+    {
+        callbacks[flag].push_back(func);
+    }
+
+    void setFlagFlightPhase(const FlightPhases& flag, bool isEnable)
+    {
+        flagsFlightPhases[flag] = isEnable;
+    }
+
+    void processFlags(const SimulatorData& hil_flags)
+    {
+        std::vector<FlightPhases> changed_flags;
+
+        processFlagsImpl(hil_flags, changed_flags);
+
+        /* calling the callbacks subscribed to the changed flags */
+        for (unsigned int i = 0; i < changed_flags.size(); i++)
+        {
+            for (const auto& callback : callbacks[changed_flags[i]])
+            {
+                callback();
+            }
+        }
+
+        prev_flagsFlightPhases = flagsFlightPhases;
+    }
+
+    virtual void printOutcomes() = 0;
+
+protected:
+    virtual void processFlagsImpl(const SimulatorData& hil_flags,
+                                  std::vector<FlightPhases>& changed_flags) = 0;
+
+    virtual void handleEventImpl(const Event& e,
+                                 std::vector<FlightPhases>& changed_flags) = 0;
+
+    void handleEvent(const Boardcore::Event& e) override
+    {
+        std::vector<FlightPhases> changed_flags;
+
+        handleEventImpl(e, changed_flags);
+
+        /* calling the callbacks subscribed to the changed flags */
+        for (unsigned int i = 0; i < changed_flags.size(); i++)
+        {
+            for (const auto& callback : callbacks[changed_flags[i]])
+            {
+                callback();
+            }
+        }
+
+        prev_flagsFlightPhases = flagsFlightPhases;
+    }
+
+    void registerOutcomes(const FlightPhases& phase)
+    {
+        TimedTrajectoryPoint temp = getCurrentPosition();
+        outcomes[phase]           = Outcomes(temp.z, temp.vz);
+    }
+
+    bool isSetTrue(const FlightPhases& phase) const
+    {
+        return flagsFlightPhases.at(phase) && !prev_flagsFlightPhases.at(phase);
+    }
+
+    bool isSetFalse(const FlightPhases& phase) const
+    {
+        return !flagsFlightPhases.at(phase) && prev_flagsFlightPhases.at(phase);
+    }
+
+    std::map<FlightPhases, bool> flagsFlightPhases;
+    std::map<FlightPhases, bool> prev_flagsFlightPhases;
+    std::map<FlightPhases, std::vector<PhasesCallback>> callbacks;
+    std::map<FlightPhases, Outcomes> outcomes;
+};
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/hil/HILTransceiver.h b/src/shared/hil/HILTransceiver.h
new file mode 100644
index 0000000000000000000000000000000000000000..dbd402ef945822c7be6aa5e4879ea6bab5453c6e
--- /dev/null
+++ b/src/shared/hil/HILTransceiver.h
@@ -0,0 +1,214 @@
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <ActiveObject.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <drivers/usart/USART.h>
+#include <utils/Debug.h>
+
+#include "HIL.h"
+#include "drivers/usart/USART.h"
+
+namespace Boardcore
+{
+
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HIL;
+
+class HILTransceiverBase : public ActiveObject
+{
+public:
+    /**
+     * @brief Construct a serial connection attached to a control algorithm
+     */
+    explicit HILTransceiverBase(USART &hilSerial) : hilSerial(hilSerial) {}
+
+    /**
+     * @brief Returns the number of lost updates.
+     * @return the number of updates lost due to communication with the
+     * simulator.
+     */
+    int getLostUpdates() const { return nLostUpdates; }
+
+    /**
+     * @brief Returns the value in ns of the timestamp of the last received
+     * simulatorData.
+     */
+    int64_t getTimestampSimulatorData() const { return timestampSimulatorData; }
+
+protected:
+    /**
+     * @brief Waits for the control algorithm(s) to update actuatorData.
+     */
+    void waitActuatorData()
+    {
+        miosix::Lock<miosix::FastMutex> l(mutex);
+        while (!updated)
+        {
+            condVar.wait(l);
+        }
+        updated = false;
+    }
+
+    USART &hilSerial;
+    bool receivedFirstPacket       = false;
+    bool updated                   = false;
+    int nLostUpdates               = 0;
+    int64_t timestampSimulatorData = 0;  // timestamp of the last received
+                                         // simulatorData [ns]
+    miosix::FastMutex mutex;
+    miosix::ConditionVariable condVar;
+    PrintLogger logger = Logging::getLogger("HILTransceiver");
+};
+
+/**
+ * @brief HILTransceiver is a Singleton and provides an easy interface for
+ * the control algorithms to send and receive data during a simulation
+ */
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HILTransceiver : public HILTransceiverBase
+{
+public:
+    /**
+     * @brief Construct a serial connection attached to a control algorithm.
+     *
+     * @param hilSerial Serial port for the HIL communication.
+     */
+    explicit HILTransceiver(USART &hilSerial,
+                            HILPhasesManager<FlightPhases, SimulatorData,
+                                             ActuatorData> *hilPhasesManager)
+        : HILTransceiverBase(hilSerial), actuatorData(),
+          hilPhasesManager(hilPhasesManager)
+    {
+    }
+
+    /**
+     * @brief sets the actuator data and then wakes up the
+     * MatlabTransceiver thread in order to send the data back to the
+     * simulator (called by the control algorithm)
+     * @param actuatorData sets the data that will be sent to the
+     * simulator
+     */
+    void setActuatorData(ActuatorData actuatorData)
+    {
+        miosix::Lock<miosix::FastMutex> l(mutex);
+
+        // If already updated increment lost updates
+        if (updated)
+        {
+            nLostUpdates++;
+        }
+
+        this->actuatorData = actuatorData;
+        updated            = true;
+        condVar.signal();
+    }
+
+    /**
+     * @brief returns the reference of the SimulatorData
+     *
+     * @return reference to the data simulated by matlab
+     */
+    const SimulatorData *getSensorData() const { return &simulatorData; }
+
+private:
+    void run() override;
+
+    SimulatorData simulatorData;
+    ActuatorData actuatorData;
+    HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>
+        *hilPhasesManager;
+};
+
+/**
+ * @brief The thread deals with the communication between the simulator and the
+ * board.
+ *
+ * After the first time the data is received, the loop of this thread:
+ * - Reads the simulated data and copies them in the SensorData structure;
+ * - Notifies every sensor that new data arrived;
+ * - Waits for the control algorithms to update the actuator data;
+ * - Sends back the value to the simulator.
+ */
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+void HILTransceiver<FlightPhases, SimulatorData, ActuatorData>::run()
+{
+    LOG_INFO(logger, "HIL Transceiver started");
+    hilSerial.clearQueue();
+
+    miosix::led2On();
+    hilSerial.write(&actuatorData, sizeof(ActuatorData));
+    miosix::led2Off();
+
+    while (!shouldStop())
+    {
+        // Pausing the kernel in order to copy the data in the shared structure
+        {
+            SimulatorData tempData;
+            nLostUpdates = 0;
+            miosix::led3On();
+            size_t nRead = 0;
+            if (!hilSerial.readBlocking(&tempData, sizeof(SimulatorData),
+                                        nRead))
+            {
+                LOG_ERR(logger, "Failed serial read");
+            }
+
+            assert(nRead == sizeof(SimulatorData) &&
+                   "Read less then SimulatorData bytes");
+
+            hilSerial.clearQueue();
+            miosix::led3Off();
+
+            miosix::PauseKernelLock kLock;
+            simulatorData          = tempData;
+            timestampSimulatorData = miosix::getTime();
+        }
+
+        // If this is the first packet to be received, then update the flight
+        // phase manager
+        if (!receivedFirstPacket)
+        {
+            receivedFirstPacket = true;
+            hilPhasesManager->simulationStarted();
+        }
+
+        // Trigger events relative to the flight phases
+        hilPhasesManager->processFlags(simulatorData);
+
+        if (nLostUpdates > 0)
+        {
+            // This means also that the number of samples used for the mean sent
+            // to the HIL simulator is made up of more than the number of
+            // samples we though
+            LOG_WARN(logger, "%d Lost updates", nLostUpdates);
+        }
+
+        waitActuatorData();
+        miosix::led2On();
+        hilSerial.write(&actuatorData, sizeof(ActuatorData));
+        miosix::led2Off();
+    }
+}
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 70e6426fbbdcab9915279a870c77cce2cf9296b5..678b4d59cb368d34a2fa8db25e6c128f0a7be112 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -53,6 +53,7 @@
 #include <sensors/LPS22DF/LPS22DFData.h>
 #include <sensors/LPS28DFW/LPS28DFWData.h>
 #include <sensors/LSM6DSRX/LSM6DSRXData.h>
+#include <sensors/MAX31856/MAX31856Data.h>
 #include <sensors/MBLoadCell/MBLoadCellData.h>
 #include <sensors/MPU9250/MPU9250Data.h>
 #include <sensors/MS5803/MS5803Data.h>
@@ -144,6 +145,7 @@ void registerTypes(Deserializer& ds)
     ds.registerType<LSM6DSRXData>();
     ds.registerType<MagnetometerData>();
     ds.registerType<MavlinkStatus>();
+    ds.registerType<MAX31856Data>();
     ds.registerType<MBLoadCellData>();
     ds.registerType<MPU9250Data>();
     ds.registerType<MPXH6115AData>();
diff --git a/src/shared/sensors/ADS1118/ADS1118.h b/src/shared/sensors/ADS1118/ADS1118.h
index dfdb2bab87f1431518c45a121c4540fde5aac831..828cdc9bb08024c3a933e0714ac6f2e505afa24c 100644
--- a/src/shared/sensors/ADS1118/ADS1118.h
+++ b/src/shared/sensors/ADS1118/ADS1118.h
@@ -281,7 +281,7 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     /**
      * @brief Reads the previously configured channel while writing the next
      * enabled configuration.
@@ -290,6 +290,7 @@ private:
      */
     ADS1118Data sampleImpl() override;
 
+private:
     /**
      * @brief Writes the configuration specified, reads the previous written
      * configuration's value and stores it. If enabled checks also that the
diff --git a/src/shared/sensors/ADS131M04/ADS131M04.h b/src/shared/sensors/ADS131M04/ADS131M04.h
index d8142f18c33e9e28e3321f82f4158fcf02985bc4..2d4e3adb51ec28977db7804501cf7a20f3b044e5 100644
--- a/src/shared/sensors/ADS131M04/ADS131M04.h
+++ b/src/shared/sensors/ADS131M04/ADS131M04.h
@@ -124,9 +124,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     ADS131M04Data sampleImpl() override;
 
+private:
     /**
      * @brief Overwrites the channel settings.
      *
diff --git a/src/shared/sensors/ADS131M08/ADS131M08.h b/src/shared/sensors/ADS131M08/ADS131M08.h
index a83fdaf0978013227e8990c83a7540ce7268cf9d..41f049a580a53456639bfaf45d609a6bd3e70534 100644
--- a/src/shared/sensors/ADS131M08/ADS131M08.h
+++ b/src/shared/sensors/ADS131M08/ADS131M08.h
@@ -124,9 +124,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     ADS131M08Data sampleImpl() override;
 
+private:
     /**
      * @brief Overwrites the channel settings.
      *
diff --git a/src/shared/sensors/BME280/BME280.h b/src/shared/sensors/BME280/BME280.h
index 1e7e9f1b764dee8da541f2e0294d4ee626eeb551..99cdb4017719ec3396a10de5c35addc3c01218c2 100644
--- a/src/shared/sensors/BME280/BME280.h
+++ b/src/shared/sensors/BME280/BME280.h
@@ -238,9 +238,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     BME280Data sampleImpl() override;
 
+private:
     void reset();
 
     void setConfiguration();
diff --git a/src/shared/sensors/BME280/BME280I2C.h b/src/shared/sensors/BME280/BME280I2C.h
index fcc512bdb16ae7210753fdb2f454273f09f4df6f..3c6f76cad66f430a1f49573b4eb241d286c0c881 100644
--- a/src/shared/sensors/BME280/BME280I2C.h
+++ b/src/shared/sensors/BME280/BME280I2C.h
@@ -238,9 +238,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     BME280Data sampleImpl() override;
 
+private:
     bool reset();
 
     void setConfiguration();
diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h
index 8ba79d6f49f2ba7c2faac1fd91d95ea7dbf3d415..3b7abf775d1d0cd9aa930891d61729cf15f8f2cd 100644
--- a/src/shared/sensors/BMP280/BMP280.h
+++ b/src/shared/sensors/BMP280/BMP280.h
@@ -216,9 +216,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     BMP280Data sampleImpl() override;
 
+private:
     void reset();
 
     void setConfiguration();
diff --git a/src/shared/sensors/BMP280/BMP280I2C.h b/src/shared/sensors/BMP280/BMP280I2C.h
index 1c682f6a290f7fb821ab965a36ed36724db77c9f..bf1f5e0e1f905ae2e3bdb633968092f23c01c7cf 100644
--- a/src/shared/sensors/BMP280/BMP280I2C.h
+++ b/src/shared/sensors/BMP280/BMP280I2C.h
@@ -216,9 +216,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     BMP280Data sampleImpl() override;
 
+private:
     bool reset();
 
     void setConfiguration();
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h
index e805592cd35b300b4866c562c462093a13a7a374..1be3ae19390c8b7cf8e87bc4dba0126dba0389ce 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.h
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h
@@ -60,9 +60,10 @@ public:
 
     void stopCalibration();
 
-private:
+protected:
     BMX160WithCorrectionData sampleImpl() override;
 
+private:
     /**
      * @brief Rotates data axes as specified.
      */
diff --git a/src/shared/sensors/HILSensor.h b/src/shared/sensors/HILSensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..d69bf914910e8ad9fac266b0c8408d20e2c98483
--- /dev/null
+++ b/src/shared/sensors/HILSensor.h
@@ -0,0 +1,144 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Emilio Corigliano
+ *
+ * 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 <miosix.h>
+
+#include <type_traits>
+
+#include "HILSimulatorData.h"
+#include "Sensor.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Class that wraps a real sensor to perform HIL simulations.
+ *
+ * This class extends a real sensor class T. For the real sensor construction is
+ * used the move constructor of an already created T sensor. The sampleImpl
+ * method is overridden in order to expose the sample returned by the updateData
+ * function passed in the constructor.
+ * In this way, the corresponding HILSensor behaves exactly like a sensor T, but
+ * returns the data given by the simulator.
+ *
+ * The real hardware sensor can be used by creating an instance of this class
+ * with the `enableHw` constructor parameter set to `true`. In that case,
+ * when the `sample` method is called, the real sensor is sampled and then
+ * the data from the simulator is mapped into the SensorData struct.
+ */
+
+template <class T>
+class HILSensor : public T
+{
+public:
+    // A requirement is that the class T must be move constructible
+    static_assert(std::is_move_constructible<T>::value,
+                  "T must be move constructible!");
+    // A requirement is that the class T must be a Sensor
+    static_assert(std::is_base_of<Sensor<typename T::DataType>, T>::value,
+                  "T must inherit from Sensor<T::DataType>");
+
+    using UpdateFn = std::function<typename T::DataType(void)>;
+
+    /**
+     * @brief Constructor of the HILSensor which decorates the passed sensor
+     * with the sampling of the simulator data.
+     *
+     * @param sensor The sensor to be wrapped.
+     * @param enableHw Flag to enable the sampling of the real hardware.
+     * @param updateData Function that maps the data from the simulator data to
+     * the sensor current sample.
+     */
+    HILSensor(T&& sensor, bool enableHw, UpdateFn updateData)
+        : T{std::move(sensor)}, enableHw{enableHw}, updateData{
+                                                        std::move(updateData)}
+    {
+    }
+
+    bool init() override { return (enableHw ? T::init() : true); }
+
+    bool selfTest() override { return (enableHw ? T::selfTest() : true); }
+
+    /**
+     * @brief Overridden sampleImpl method so that the data could be actually
+     * taken from the simulator.
+     *
+     * If the hardware is enabled, it also samples the real sensor.
+     *
+     * @return The data struct with the current simulator sample.
+     */
+    typename T::DataType sampleImpl() override
+    {
+        if (enableHw)
+        {
+            auto realSample = T::sampleImpl();
+            {
+                miosix::Lock<miosix::FastMutex> l(mutexRealData);
+                lastRealSample = realSample;
+            }
+        }
+
+        return updateData();
+    }
+
+    /**
+     * @brief Method analogous to getLastSample but returns the real sample
+     * taken from the hardware sensor.
+     *
+     * @return The data struct with the current sensor sample.
+     */
+    typename T::DataType getRealLastSample()
+    {
+        miosix::Lock<miosix::FastMutex> l(mutexRealData);
+        return lastRealSample;
+    }
+
+protected:
+    typename T::DataType lastRealSample;  // Last sample of the real sensor
+    bool enableHw;                        // Flag to enable hardware sampling
+    UpdateFn updateData;  // Function to get the lastSample from simulatorData
+
+    // Thread safe mutex to synchronize writes and reads on lastRealSample
+    miosix::FastMutex mutexRealData;
+};
+
+/**
+ * @brief Static method that hillificates sensors, turning a real sensor in
+ * a HILSensor.
+ *
+ * Creates a HILSensor of the sensor type T. The passed sensor must be an
+ * already instantiated sensor.
+ */
+template <typename T>
+static void hillificator(std::unique_ptr<T>& sensor, bool enableHw,
+                         typename HILSensor<T>::UpdateFn updateData)
+{
+    if (sensor)
+    {
+        sensor = std::make_unique<Boardcore::HILSensor<T>>(
+            std::move(*sensor), enableHw, std::move(updateData));
+    }
+}
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/sensors/HILSimulatorData.h b/src/shared/sensors/HILSimulatorData.h
new file mode 100644
index 0000000000000000000000000000000000000000..dacd6898f5102b06d2b5bb7f470901733a31fcf5
--- /dev/null
+++ b/src/shared/sensors/HILSimulatorData.h
@@ -0,0 +1,84 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <Eigen/Core>
+#include <ostream>
+
+namespace Boardcore
+{
+
+template <int N_DATA>
+struct AccelerometerSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float measures[N_DATA][3];
+};
+
+template <int N_DATA>
+struct GyroscopeSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float measures[N_DATA][3];
+};
+
+template <int N_DATA>
+struct MagnetometerSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float measures[N_DATA][3];
+};
+
+template <int N_DATA>
+struct BarometerSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float measures[N_DATA];
+};
+
+template <int N_DATA>
+struct GPSSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float positionMeasures[N_DATA][3];
+    float velocityMeasures[N_DATA][3];
+    float fix;
+    float num_satellites;
+};
+
+template <int N_DATA>
+struct PitotSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float deltaP[N_DATA];
+    float staticPressure[N_DATA];
+};
+
+template <int N_DATA>
+struct TemperatureSimulatorData
+{
+    static constexpr int NDATA = N_DATA;
+    float measures[N_DATA];
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/HX711/HX711.h b/src/shared/sensors/HX711/HX711.h
index 4674ea16e84c255470bb80ec54aebb997d624665..460b8ccacea99247940b44b75a4dd487f4b3da68 100644
--- a/src/shared/sensors/HX711/HX711.h
+++ b/src/shared/sensors/HX711/HX711.h
@@ -112,9 +112,10 @@ public:
      */
     float getOffset();
 
-private:
+protected:
     HX711Data sampleImpl() override;
 
+private:
     SPIBusInterface& bus;
     miosix::GpioPin sckPin;
     const SPIBusConfig config;
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h
index d7a93eb2bfc9dc4a56cdc69cebb1b5779058b179..7e781f9557c2cea4ad0d446bf33d32ea50e7c8b1 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.h
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h
@@ -99,9 +99,10 @@ public:
      */
     bool applyConfig(Config config);
 
-private:
+protected:
     LIS2MDLData sampleImpl() override;
 
+private:
     SPISlave slave;
     Config configuration;
 
diff --git a/src/shared/sensors/LIS331HH/LIS331HH.h b/src/shared/sensors/LIS331HH/LIS331HH.h
index 252e7bf375a40563f9cab47181c704e296ddf821..8df1374b54cfa1e3c4ec91ed950e9dde26f0b5aa 100644
--- a/src/shared/sensors/LIS331HH/LIS331HH.h
+++ b/src/shared/sensors/LIS331HH/LIS331HH.h
@@ -58,9 +58,10 @@ public:
 
     void setFullScaleRange(FullScaleRange fs);
 
-private:
+protected:
     LIS331HHData sampleImpl() override;
 
+private:
     SPISlave slave;
     float sensitivity = 6.0 / 32767.0;
 
diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.h b/src/shared/sensors/LIS3MDL/LIS3MDL.h
index ed98e2768a85097cdde7ea0e13b74fbc83d56289..2e0a5d90514b563930a3ef55532ceb86111fbf5a 100644
--- a/src/shared/sensors/LIS3MDL/LIS3MDL.h
+++ b/src/shared/sensors/LIS3MDL/LIS3MDL.h
@@ -145,9 +145,10 @@ public:
      */
     bool applyConfig(Config config);
 
-private:
+protected:
     LIS3MDLData sampleImpl() override;
 
+private:
     void updateUnit(FullScale fs);
 
     SPISlave slave;
diff --git a/src/shared/sensors/LPS22DF/LPS22DF.h b/src/shared/sensors/LPS22DF/LPS22DF.h
index 876c5fc499d2ea303754df8e0f4abe28a7e4fc6b..63a761dd67494cb64acfc27a97d9ed0359043e06 100644
--- a/src/shared/sensors/LPS22DF/LPS22DF.h
+++ b/src/shared/sensors/LPS22DF/LPS22DF.h
@@ -138,9 +138,10 @@ public:
      */
     void setOutputDataRate(ODR odr);
 
-private:
+protected:
     LPS22DFData sampleImpl() override;
 
+private:
     SPISlave slave;
     Config config;
 
diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.h b/src/shared/sensors/LPS28DFW/LPS28DFW.h
index 33012760c5b6eeac46f921c3bee81ab5af94ccd0..2f2cf5010433758e198c986f9ba50e549cd1dec0 100644
--- a/src/shared/sensors/LPS28DFW/LPS28DFW.h
+++ b/src/shared/sensors/LPS28DFW/LPS28DFW.h
@@ -154,9 +154,10 @@ public:
      */
     bool setDRDYInterrupt(bool drdy);
 
-private:
+protected:
     LPS28DFWData sampleImpl() override;
 
+private:
     /**
      * @brief Converting the bytes read from the sensor to the pressure in Pa.
      */
diff --git a/src/shared/sensors/LPS331AP/LPS331AP.h b/src/shared/sensors/LPS331AP/LPS331AP.h
index 2af56dceecb6a3dfe6bc98ea0e4b706b1c836a15..3a0bc3c1c4b7bb4c60e9a827f95f03f264d887a8 100644
--- a/src/shared/sensors/LPS331AP/LPS331AP.h
+++ b/src/shared/sensors/LPS331AP/LPS331AP.h
@@ -48,9 +48,10 @@ public:
 
     bool selfTest() override;
 
-private:
+protected:
     LPS331APData sampleImpl() override;
 
+private:
     bool checkWhoAmI();
 
     static constexpr uint8_t WHO_AM_I_VAL = 0x58;  ///< Who am I value
diff --git a/src/shared/sensors/MAX31855/MAX31855.h b/src/shared/sensors/MAX31855/MAX31855.h
index fcfcd412d6234e325489f2f61fbfab244ce01dec..9391e609912fb712c7c850bd0304c43a559986a3 100644
--- a/src/shared/sensors/MAX31855/MAX31855.h
+++ b/src/shared/sensors/MAX31855/MAX31855.h
@@ -64,9 +64,10 @@ public:
      */
     TemperatureData readInternalTemperature();
 
-private:
+protected:
     TemperatureData sampleImpl() override;
 
+private:
     SPISlave slave;
 
     PrintLogger logger = Logging::getLogger("max31855");
diff --git a/src/shared/sensors/MAX31856/MAX31856.h b/src/shared/sensors/MAX31856/MAX31856.h
index 7e329e28222d1b0c46a049bba3580f2604bb4843..1b4a408ba10c2c50861bbdea8e630c7770b88036 100644
--- a/src/shared/sensors/MAX31856/MAX31856.h
+++ b/src/shared/sensors/MAX31856/MAX31856.h
@@ -82,9 +82,10 @@ public:
 
     void setColdJunctionOffset(float offset);
 
-private:
+protected:
     MAX31856Data sampleImpl() override;
 
+private:
     SPISlave slave;
     ThermocoupleType type;
 
diff --git a/src/shared/sensors/MAX6675/MAX6675.h b/src/shared/sensors/MAX6675/MAX6675.h
index 0ee3e01bdf07bf807ee76a6287c80d7cb6d53733..743a46fc8b33a53ca760cb10641462d9eef6e887 100644
--- a/src/shared/sensors/MAX6675/MAX6675.h
+++ b/src/shared/sensors/MAX6675/MAX6675.h
@@ -55,9 +55,10 @@ public:
      */
     bool checkConnection();
 
-private:
+protected:
     TemperatureData sampleImpl() override;
 
+private:
     const SPISlave slave;
 
     PrintLogger logger = Logging::getLogger("max6675");
diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h
index 5d262adc7f8fe83879d552b6db57af055b93a9d3..c0caf5f8d5ad3c1e13f1f0f0f0b558a4fd06415b 100644
--- a/src/shared/sensors/MPU9250/MPU9250.h
+++ b/src/shared/sensors/MPU9250/MPU9250.h
@@ -220,9 +220,10 @@ public:
      */
     bool selfTest() override { return true; };
 
-private:
+protected:
     MPU9250Data sampleImpl() override;
 
+private:
     void resetDevice();
 
     void selectAutoClock();
diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h
index 7147d7ffeeeedb61484c1ea8e9feb133256da5c1..5fa1466bc11a0672484f98d22eeebd80a7de1a14 100644
--- a/src/shared/sensors/MS5803/MS5803.h
+++ b/src/shared/sensors/MS5803/MS5803.h
@@ -79,7 +79,7 @@ public:
 
     bool selfTest() override;
 
-private:
+protected:
     /**
      * Implements a state machines composed of 3 states:
      * 1. Command pressure sample
@@ -94,6 +94,7 @@ private:
      */
     MS5803Data sampleImpl() override;
 
+private:
     MS5803Data updateData();
 
     SPISlave spiSlave;
diff --git a/src/shared/sensors/MS5803/MS5803I2C.h b/src/shared/sensors/MS5803/MS5803I2C.h
index 96db302d636c1e11143b1faf5e6758df4e23b087..3c983fbf1653e4b9f521d732f515f562013745d1 100644
--- a/src/shared/sensors/MS5803/MS5803I2C.h
+++ b/src/shared/sensors/MS5803/MS5803I2C.h
@@ -78,7 +78,7 @@ public:
 
     bool selfTest() override;
 
-private:
+protected:
     /**
      * Implements a state machines composed of 3 states:
      * 1. Command pressure sample
@@ -93,6 +93,7 @@ private:
      */
     MS5803Data sampleImpl() override;
 
+private:
     MS5803Data updateData();
 
     uint16_t readReg(uint8_t reg);
diff --git a/src/shared/sensors/Sensor.h b/src/shared/sensors/Sensor.h
index f26781e0ab931c62bd2926f5535296c0d8f65c98..bfbae274092d983a504ddc459b9f49b8a78fc5c9 100644
--- a/src/shared/sensors/Sensor.h
+++ b/src/shared/sensors/Sensor.h
@@ -1,5 +1,5 @@
 /* Copyright (c) 2016-2020 Skyward Experimental Rocketry
- * Authors: Alain Carlucci, Luca Conterio
+ * Authors: Alain Carlucci, Luca Conterio, Davide Mor, Emilio Corigliano
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -25,6 +25,7 @@
 #include <miosix.h>
 
 #include <array>
+#include <memory>
 #include <type_traits>
 
 #include "SensorData.h"
@@ -88,8 +89,11 @@ public:
 template <typename Data>
 class Sensor : public virtual AbstractSensor
 {
+public:
+    using DataType = Data;
+
 protected:
-    Data lastSample;
+    DataType lastSample;
 
     /**
      * @brief Read a data sample from the sensor.
@@ -97,19 +101,23 @@ protected:
      *        available correct sample.
      * @return sensor data sample
      */
-    virtual Data sampleImpl() = 0;
+    virtual DataType sampleImpl() = 0;
 
     // Thread safe mutex to synchronize writes and reads
     miosix::FastMutex mutex;
 
 public:
+    Sensor() {}
+
+    Sensor(Sensor&& other) : lastSample{std::move(other.lastSample)}, mutex{} {}
+
     virtual ~Sensor() {}
 
     void sample() override
     {
         // Sampling outside of the protected zone ensures that the sampling
         // action cannot cause locks or delays
-        Data d = sampleImpl();
+        DataType d = sampleImpl();
 
         {
             miosix::Lock<miosix::FastMutex> l(mutex);
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
index 733d2ed6ca2893abf4eb22e9ec2db96ed75885f5..9187466a692e354a3454855c7d8f43073835a845 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
@@ -81,9 +81,10 @@ public:
      */
     bool selfTest() override;
 
-private:
+protected:
     UBXGPSData sampleImpl() override;
 
+private:
     /**
      * @brief Resets the device to its default configuration.
      *
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.h b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
index e36b6d5b431ab603acae63f597e5e46a95c7ac6f..eb14fb4e274abb9b6f3a729395407204d029f887 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSpi.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
@@ -74,9 +74,10 @@ public:
 
     bool selfTest() override;
 
-private:
+protected:
     UBXGPSData sampleImpl() override;
 
+private:
     /**
      * @brief Resets the device to its default configuration.
      *
diff --git a/src/shared/sensors/VN100/VN100Serial.h b/src/shared/sensors/VN100/VN100Serial.h
index cd0959f743c0efef266f30d92165c7de5fc2786f..36ce97ac473533c8a33801546b5f40d841cd36b1 100644
--- a/src/shared/sensors/VN100/VN100Serial.h
+++ b/src/shared/sensors/VN100/VN100Serial.h
@@ -116,12 +116,13 @@ public:
 
     bool selfTest() override;
 
-private:
+protected:
     /**
      * @brief Sample action implementation.
      */
     VN100SerialData sampleImpl() override;
 
+private:
     /**
      * @brief Active object method, about the thread execution
      */
diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h
index e7baf675b592dcdae54029c084f2bd7c1a8536d0..1e82396f0933adf973f100a4f6fb577f6b164b92 100644
--- a/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-103kPa range (0-15psi)
  */
-class HSCMAND015PA final : public HoneywellPressureSensor<HSCMAND015PAData>
+class HSCMAND015PA : public HoneywellPressureSensor<HSCMAND015PAData>
 {
 public:
     HSCMAND015PA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h
index 7669f3f4f686ea4e024533f6d90d35384945f20a..1f207637d243f1b9016f0df94be200f6be0542ef 100644
--- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-103kPa range (0-15psi)
  */
-class HSCMRNN015PA final : public HoneywellPressureSensor<HSCMRNN015PAData>
+class HSCMRNN015PA : public HoneywellPressureSensor<HSCMRNN015PAData>
 {
 public:
     HSCMRNN015PA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h
index e35a3f86eb841652bd726958358b1f7f28fbbcf3..2b1f3a25a241d27273547e5dde4c7e5ac6ed234c 100644
--- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi)
  */
-class HSCMRNN030PA final : public HoneywellPressureSensor<HSCMRNN030PAData>
+class HSCMRNN030PA : public HoneywellPressureSensor<HSCMRNN030PAData>
 {
 public:
     HSCMRNN030PA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h
index f2c17aa74bfc4fdd11519a322f7fec56494a6524..7b34286855ae7902eb18c8e0bb024d5d6f04e506 100644
--- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-160kPa range
  */
-class HSCMRNN160KA final : public HoneywellPressureSensor<HSCMRNN160KAData>
+class HSCMRNN160KA : public HoneywellPressureSensor<HSCMRNN160KAData>
 {
 public:
     HSCMRNN160KA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h
index d18cb5fa08952a8b1999428fde5eb4404b7c066e..7cbbe1601aec2166037fa824f0039eae54728420 100644
--- a/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi)
  */
-class SSCDANN030PAA final : public HoneywellPressureSensor<SSCDANN030PAAData>
+class SSCDANN030PAA : public HoneywellPressureSensor<SSCDANN030PAAData>
 {
 public:
     SSCDANN030PAA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
index b144f93344813b9a9a60f18b6d7cdd2fefc30ba1..4659631f014e5f0fdc3c2af4b02009892c0c0227 100644
--- a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
@@ -33,7 +33,7 @@ namespace Boardcore
 /**
  * @brief Differential pressure sensor with a ±103kPa range (±15psi)
  */
-class SSCDRRN015PDA final : public HoneywellPressureSensor<SSCDRRN015PDAData>
+class SSCDRRN015PDA : public HoneywellPressureSensor<SSCDRRN015PDAData>
 {
 public:
     SSCDRRN015PDA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h b/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h
index beaac6843465e6112b8f9348d11b1dde73636659..3cd4ab0580676a54c9f0cc70efdd7ce64a21fe0d 100644
--- a/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h
@@ -31,7 +31,7 @@ namespace Boardcore
 /**
  * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi)
  */
-class SSCMRNN030PA final : public HoneywellPressureSensor<SSCMRNN030PAData>
+class SSCMRNN030PA : public HoneywellPressureSensor<SSCMRNN030PAData>
 {
 public:
     SSCMRNN030PA(std::function<ADCData()> getSensorVoltage,
diff --git a/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h b/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h
index d72485d906ec4fbbd8d6d9a882b0ff5355b53e57..e55cd7c81fe1b684ea2ae9b20e4f57e033f3bc88 100644
--- a/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h
+++ b/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h
@@ -33,7 +33,7 @@ namespace Boardcore
 /**
  * @brief Driver for NXP's MPXH6115A pressure sensor
  */
-class MPXH6115A final : public AnalogPressureSensor<MPXH6115AData>
+class MPXH6115A : public AnalogPressureSensor<MPXH6115AData>
 {
 public:
     MPXH6115A(std::function<ADCData()> getVoltage,
diff --git a/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h b/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h
index ab136e27eddc8b19a92af746ec273d8340711aec..fdb1d02fb7b938f8ee3ab2e68c1a21c93a52d0b0 100644
--- a/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h
+++ b/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h
@@ -33,7 +33,7 @@ namespace Boardcore
 /**
  * @brief Driver for NXP's MPXHZ6130A pressure sensor
  */
-class MPXH6400A final : public AnalogPressureSensor<MPXH6400AData>
+class MPXH6400A : public AnalogPressureSensor<MPXH6400AData>
 {
 public:
     MPXH6400A(std::function<ADCData()> getVoltage,
diff --git a/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h b/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h
index f2beffc5d4466bfed53d9ec5f2a719ac325a6947..26e0ab5b1efc9bcb8fec9e411f8a5c8af3445767 100644
--- a/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h
+++ b/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h
@@ -33,7 +33,7 @@ namespace Boardcore
 /**
  * @brief Driver for NXP's MPXHZ6130A pressure sensor
  */
-class MPXHZ6130A final : public AnalogPressureSensor<MPXHZ6130AData>
+class MPXHZ6130A : public AnalogPressureSensor<MPXHZ6130AData>
 {
 public:
     MPXHZ6130A(std::function<ADCData()> getVoltage,
diff --git a/src/tests/hil/BoardScheduler.cpp b/src/tests/hil/BoardScheduler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8acb5a4f1e0fdb01bf46e1552cdbe0b7489306f
--- /dev/null
+++ b/src/tests/hil/BoardScheduler.cpp
@@ -0,0 +1,65 @@
+/* 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 "BoardScheduler.h"
+
+using namespace Boardcore;
+
+namespace HILTest
+{
+BoardScheduler::BoardScheduler()
+{
+    scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
+    scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
+    scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
+    scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
+}
+
+TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
+{
+    switch (priority.get())
+    {
+        case miosix::PRIORITY_MAX:
+            return scheduler4;
+        case miosix::PRIORITY_MAX - 1:
+            return scheduler3;
+        case miosix::PRIORITY_MAX - 2:
+            return scheduler2;
+        case miosix::MAIN_PRIORITY:
+            return scheduler1;
+        default:
+            return scheduler1;
+    }
+}
+
+bool BoardScheduler::start()
+{
+    return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
+           scheduler4->start();
+}
+
+bool BoardScheduler::isStarted()
+{
+    return scheduler1->isRunning() && scheduler2->isRunning() &&
+           scheduler3->isRunning() && scheduler4->isRunning();
+}
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/BoardScheduler.h b/src/tests/hil/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..5664a8095336cdcfdcf03e83d0f9fde7e4b16838
--- /dev/null
+++ b/src/tests/hil/BoardScheduler.h
@@ -0,0 +1,62 @@
+/* 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 <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace HILTest
+{
+/**
+ * @brief Class that wraps the 4 main task schedulers of the entire OBSW.
+ * There is a task scheduler for every miosix priority
+ */
+class BoardScheduler : public Boardcore::Module
+{
+public:
+    BoardScheduler();
+
+    /**
+     * @brief Get the Scheduler object relative to the requested priority
+     *
+     * @param priority The task scheduler priority
+     * @return Boardcore::TaskScheduler& Reference to the requested task
+     * scheduler.
+     * @note Min priority scheduler is returned in case of non valid priority.
+     */
+    Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Returns if all the schedulers are up and running
+     */
+    bool isStarted();
+
+private:
+    Boardcore::TaskScheduler* scheduler1;
+    Boardcore::TaskScheduler* scheduler2;
+    Boardcore::TaskScheduler* scheduler3;
+    Boardcore::TaskScheduler* scheduler4;
+};
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/Buses.h b/src/tests/hil/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..33dffab577e4cee189b06da57ffb5b7abed0c50c
--- /dev/null
+++ b/src/tests/hil/Buses.h
@@ -0,0 +1,56 @@
+/* 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 HILTest
+{
+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, 256000, 1024),
+          uart4(UART4, 115200)
+    {
+    }
+};
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/HILSimulationConfig.h b/src/tests/hil/HILSimulationConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e9e218dbec5e785be9e95303c63f5bdd46f5e5f
--- /dev/null
+++ b/src/tests/hil/HILSimulationConfig.h
@@ -0,0 +1,530 @@
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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/timer/TimestampTimer.h>
+#include <drivers/usart/USART.h>
+#include <events/EventBroker.h>
+#include <hil/HIL.h>
+#include <math.h>
+#include <sensors/HILSimulatorData.h>
+#include <sensors/SensorInfo.h>
+#include <utils/Debug.h>
+#include <utils/Stats/Stats.h>
+
+#include <list>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "Buses.h"
+#include "Sensors/SensorsConfig.h"
+#include "common/Events.h"
+
+namespace HILConfig
+{
+
+constexpr bool IS_FULL_HIL = true;
+
+/** Period of simulation [ms] */
+constexpr int SIMULATION_PERIOD = 100;
+
+/** sampling periods of sensors [ms] */
+constexpr int ACCEL_PERIOD        = HILTest::SensorsConfig::LSM6DSRX_PERIOD;
+constexpr int GYRO_PERIOD         = HILTest::SensorsConfig::LSM6DSRX_PERIOD;
+constexpr int MAGN_PERIOD         = HILTest::SensorsConfig::LIS2MDL_PERIOD;
+constexpr int IMU_PERIOD          = HILTest::SensorsConfig::IMU_PERIOD;
+constexpr int ANALOG_BARO_PERIOD  = HILTest::SensorsConfig::ADS131M08_PERIOD;
+constexpr int DIGITAL_BARO_PERIOD = HILTest::SensorsConfig::LPS22DF_PERIOD;
+constexpr int TEMP_PERIOD         = SIMULATION_PERIOD;  // just one sample
+constexpr int GPS_PERIOD          = HILTest::SensorsConfig::UBXGPS_PERIOD;
+constexpr int BARO_CHAMBER_PERIOD = 20;
+constexpr int PITOT_PERIOD        = 50;
+
+static_assert((SIMULATION_PERIOD % ACCEL_PERIOD) == 0,
+              "N_DATA_ACCEL not an integer");
+static_assert((SIMULATION_PERIOD % GYRO_PERIOD) == 0,
+              "N_DATA_GYRO not an integer");
+static_assert((SIMULATION_PERIOD % MAGN_PERIOD) == 0,
+              "N_DATA_MAGN not an integer");
+static_assert((SIMULATION_PERIOD % IMU_PERIOD) == 0,
+              "N_DATA_IMU not an integer");
+static_assert((SIMULATION_PERIOD % ANALOG_BARO_PERIOD) == 0,
+              "N_DATA_ANALOG_BARO not an integer");
+static_assert((SIMULATION_PERIOD % DIGITAL_BARO_PERIOD) == 0,
+              "N_DATA_DIGITAL_BARO not an integer");
+static_assert((SIMULATION_PERIOD % BARO_CHAMBER_PERIOD) == 0,
+              "N_DATA_BARO_CHAMBER not an integer");
+static_assert((SIMULATION_PERIOD % PITOT_PERIOD) == 0,
+              "N_DATA_PITOT not an integer");
+static_assert((SIMULATION_PERIOD % TEMP_PERIOD) == 0,
+              "N_DATA_TEMP not an integer");
+static_assert((SIMULATION_PERIOD % GPS_PERIOD) == 0,
+              "N_DATA_GPS not an integer");
+
+/** Number of samples per sensor at each simulator iteration */
+constexpr int N_DATA_ACCEL        = SIMULATION_PERIOD / ACCEL_PERIOD;
+constexpr int N_DATA_GYRO         = SIMULATION_PERIOD / GYRO_PERIOD;
+constexpr int N_DATA_MAGNETO      = SIMULATION_PERIOD / MAGN_PERIOD;
+constexpr int N_DATA_IMU          = SIMULATION_PERIOD / IMU_PERIOD;
+constexpr int N_DATA_ANALOG_BARO  = SIMULATION_PERIOD / ANALOG_BARO_PERIOD;
+constexpr int N_DATA_DIGITAL_BARO = SIMULATION_PERIOD / DIGITAL_BARO_PERIOD;
+constexpr int N_DATA_BARO_CHAMBER = SIMULATION_PERIOD / BARO_CHAMBER_PERIOD;
+constexpr int N_DATA_PITOT        = SIMULATION_PERIOD / PITOT_PERIOD;
+constexpr int N_DATA_GPS          = SIMULATION_PERIOD / GPS_PERIOD;
+constexpr int N_DATA_TEMP         = SIMULATION_PERIOD / TEMP_PERIOD;
+
+// Sensors Data
+using MainAccelerometerSimulatorData =
+    Boardcore::AccelerometerSimulatorData<N_DATA_ACCEL>;
+using MainGyroscopeSimulatorData =
+    Boardcore::GyroscopeSimulatorData<N_DATA_GYRO>;
+using MainMagnetometerSimulatorData =
+    Boardcore::MagnetometerSimulatorData<N_DATA_MAGNETO>;
+using MainGPSSimulatorData = Boardcore::GPSSimulatorData<N_DATA_GPS>;
+using MainBarometerSimulatorData =
+    Boardcore::BarometerSimulatorData<N_DATA_DIGITAL_BARO>;
+using MainChamberPressureSimulatorData =
+    Boardcore::BarometerSimulatorData<N_DATA_BARO_CHAMBER>;
+using MainPitotSimulatorData = Boardcore::PitotSimulatorData<N_DATA_PITOT>;
+using MainTemperatureSimulatorData =
+    Boardcore::TemperatureSimulatorData<N_DATA_TEMP>;
+
+/**
+ * @brief ADA data sent to the simulator
+ */
+struct ADAStateHIL
+{
+    float mslAltitude    = 0;  // Altitude at mean sea level [m].
+    float aglAltitude    = 0;  // Altitude above ground level [m].
+    float verticalSpeed  = 0;  // Vertical speed [m/s].
+    float apogeeDetected = 0;  // Flag if apogee is detected [bool]
+    float updating       = 0;  //
+
+    ADAStateHIL()
+        : mslAltitude(0), aglAltitude(0), verticalSpeed(0), apogeeDetected(0),
+          updating(0)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "mslAltitude: %+.3f\n"
+            "aglAltitude: %+.3f\n"
+            "verticalSpeed: %+.3f\n"
+            "apogeeDetected: %+.3f\n"
+            "updating: %+.3f\n",
+            mslAltitude, aglAltitude, verticalSpeed, apogeeDetected, updating);
+    }
+};
+
+/**
+ * @brief NAS data sent to the simulator
+ */
+struct NASStateHIL
+{
+    float n = 0;
+    float e = 0;
+    float d = 0;
+
+    // Velocity [m/s]
+    float vn = 0;
+    float ve = 0;
+    float vd = 0;
+
+    // Attitude as quaternion
+    float qx = 0;
+    float qy = 0;
+    float qz = 0;
+    float qw = 1;
+
+    float updating = 0;  // Flag if apogee is detected [bool]
+
+    NASStateHIL()
+        : n(0), e(0), d(0), vn(0), ve(0), vd(0), qx(0), qy(0), qz(0), qw(1),
+          updating(0)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "n: %+.3f\n"
+            "e: %+.3f\n"
+            "d: %+.3f\n"
+            "vn: %+.3f\n"
+            "ve: %+.3f\n"
+            "vd: %+.3f\n"
+            "qx: %+.3f\n"
+            "qy: %+.3f\n"
+            "qz: %+.3f\n"
+            "qw: %+.3f\n"
+            "updating: %+.3f\n",
+            n, e, d, vn, ve, vd, qx, qy, qz, qw, updating);
+    }
+};
+
+/**
+ * @brief ABK data sent to the simulator
+ */
+struct AirBrakesStateHIL
+{
+    float updating = 0;  // Flag if apogee is detected [bool]
+
+    AirBrakesStateHIL() : updating(0) {}
+
+    void print() { printf("updating: %+.3f\n", updating); }
+};
+
+/**
+ * @brief MEA data sent to the simulator
+ */
+struct MEAStateHIL
+{
+    float correctedPressure = 0;
+
+    float estimatedMass   = 0;
+    float estimatedApogee = 0;
+
+    float updating = 0;  // Flag if apogee is detected [bool]
+
+    MEAStateHIL()
+        : correctedPressure(0), estimatedMass(0), estimatedApogee(0),
+          updating(0)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "correctedPressure: %+.3f\n"
+            "estimatedMass: %+.3f\n"
+            "estimatedApogee: %+.3f\n"
+            "updating: %+.3f\n",
+            correctedPressure, estimatedMass, estimatedApogee, updating);
+    }
+};
+
+struct ActuatorsStateHIL
+{
+    float airbrakesPercentage    = 0;
+    float expulsionPercentage    = 0;
+    float mainValvePercentage    = 0;
+    float ventingValvePercentage = 0;
+    float cutterState            = 0;
+
+    ActuatorsStateHIL()
+        : airbrakesPercentage(0.0f), expulsionPercentage(0.0f),
+          mainValvePercentage(0.0f), ventingValvePercentage(0.0f)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "airbrakes: %f perc\n"
+            "expulsion: %f perc\n"
+            "mainValve: %f perc\n"
+            "venting: %f perc\n"
+            "cutter: %f\n",
+            airbrakesPercentage * 100, expulsionPercentage * 100,
+            mainValvePercentage * 100, ventingValvePercentage * 100,
+            cutterState);
+    }
+};
+
+/**
+ * @brief Data structure used by the simulator in order to directly deserialize
+ * the data received
+ *
+ * This structure then is accessed by sensors and other components in order to
+ * get the data they need
+ */
+struct SimulatorData
+{
+    MainAccelerometerSimulatorData accelerometer;
+    MainGyroscopeSimulatorData gyro;
+    MainMagnetometerSimulatorData magnetometer;
+    MainGPSSimulatorData gps;
+    MainBarometerSimulatorData barometer1, barometer2, barometer3;
+    MainChamberPressureSimulatorData pressureChamber;
+    MainPitotSimulatorData pitot;
+    MainTemperatureSimulatorData temperature;
+};
+
+/**
+ * @brief Data structure expected by the simulator
+ */
+struct ActuatorData
+{
+    ADAStateHIL adaState;
+    NASStateHIL nasState;
+    AirBrakesStateHIL airBrakesState;
+    MEAStateHIL meaState;
+    ActuatorsStateHIL actuatorsState;
+
+    ActuatorData()
+        : adaState(), nasState(), airBrakesState(), meaState(), actuatorsState()
+    {
+    }
+
+    void print()
+    {
+        adaState.print();
+        nasState.print();
+        airBrakesState.print();
+        meaState.print();
+        actuatorsState.print();
+    }
+};
+
+enum MainFlightPhases
+{
+    SIM_FLYING,
+    SIM_ASCENT,
+    SIM_BURNING,
+    SIM_AEROBRAKES,
+    SIM_PARA1,
+    SIM_PARA2,
+    SIMULATION_STARTED,
+    CALIBRATION,
+    CALIBRATION_OK,
+    ARMED,
+    LIFTOFF_PIN_DETACHED,
+    LIFTOFF,
+    AEROBRAKES,
+    APOGEE,
+    PARA1,
+    PARA2,
+    SIMULATION_STOPPED
+};
+
+class MainHILTransceiver
+    : public Boardcore::HILTransceiver<MainFlightPhases, SimulatorData,
+                                       ActuatorData>,
+      public Boardcore::Module
+{
+    using Boardcore::HILTransceiver<MainFlightPhases, SimulatorData,
+                                    ActuatorData>::HILTransceiver;
+};
+
+class MainHIL
+    : public Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>,
+      public Boardcore::Module
+{
+    using Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>::HIL;
+};
+
+class MainHILPhasesManager
+    : public Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData,
+                                         ActuatorData>,
+      public Boardcore::Module
+{
+public:
+    explicit MainHILPhasesManager(
+        std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+        : Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData,
+                                      ActuatorData>(getCurrentPosition)
+    {
+        flagsFlightPhases = {{MainFlightPhases::SIM_FLYING, false},
+                             {MainFlightPhases::SIM_ASCENT, false},
+                             {MainFlightPhases::SIM_BURNING, false},
+                             {MainFlightPhases::SIM_AEROBRAKES, false},
+                             {MainFlightPhases::SIM_PARA1, false},
+                             {MainFlightPhases::SIM_PARA2, false},
+                             {MainFlightPhases::SIMULATION_STARTED, false},
+                             {MainFlightPhases::CALIBRATION, false},
+                             {MainFlightPhases::CALIBRATION_OK, false},
+                             {MainFlightPhases::ARMED, false},
+                             {MainFlightPhases::LIFTOFF_PIN_DETACHED, false},
+                             {MainFlightPhases::LIFTOFF, false},
+                             {MainFlightPhases::AEROBRAKES, false},
+                             {MainFlightPhases::APOGEE, false},
+                             {MainFlightPhases::PARA1, false},
+                             {MainFlightPhases::PARA2, false},
+                             {MainFlightPhases::SIMULATION_STOPPED, false}};
+
+        prev_flagsFlightPhases = flagsFlightPhases;
+
+        auto& eventBroker = Boardcore::EventBroker::getInstance();
+        eventBroker.subscribe(this, Common::TOPIC_ABK);
+        eventBroker.subscribe(this, Common::TOPIC_ADA);
+        eventBroker.subscribe(this, Common::TOPIC_MEA);
+        eventBroker.subscribe(this, Common::TOPIC_DPL);
+        eventBroker.subscribe(this, Common::TOPIC_CAN);
+        eventBroker.subscribe(this, Common::TOPIC_FLIGHT);
+        eventBroker.subscribe(this, Common::TOPIC_FMM);
+        eventBroker.subscribe(this, Common::TOPIC_FSR);
+        eventBroker.subscribe(this, Common::TOPIC_NAS);
+        eventBroker.subscribe(this, Common::TOPIC_TMTC);
+        eventBroker.subscribe(this, Common::TOPIC_MOTOR);
+        eventBroker.subscribe(this, Common::TOPIC_TARS);
+        eventBroker.subscribe(this, Common::TOPIC_ALT);
+    }
+
+    void processFlagsImpl(const SimulatorData& simulatorData,
+                          std::vector<MainFlightPhases>& changed_flags) override
+    {
+        // set true when the first packet from the simulator arrives
+        if (isSetTrue(MainFlightPhases::SIMULATION_STARTED))
+        {
+            t_start = Boardcore::TimestampTimer::getTimestamp();
+
+            TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n");
+            changed_flags.push_back(MainFlightPhases::SIMULATION_STARTED);
+        }
+
+        if (flagsFlightPhases[MainFlightPhases::SIM_FLYING])
+        {
+            if (isSetTrue(MainFlightPhases::SIM_FLYING))
+            {
+                registerOutcomes(MainFlightPhases::SIM_FLYING);
+                TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
+                changed_flags.push_back(MainFlightPhases::SIM_FLYING);
+            }
+        }
+    }
+
+    void printOutcomes()
+    {
+        printf("OUTCOMES: (times dt from liftoff)\n\n");
+        printf("Simulation time: %.3f [sec]\n\n",
+               (double)(t_stop - t_start) / 1000000.0f);
+
+        printf("Motor stopped burning (simulation flag): \n");
+        outcomes[MainFlightPhases::SIM_BURNING].print(t_liftoff);
+
+        printf("Airbrakes exit shadowmode: \n");
+        outcomes[MainFlightPhases::AEROBRAKES].print(t_liftoff);
+
+        printf("Apogee: \n");
+        outcomes[MainFlightPhases::APOGEE].print(t_liftoff);
+
+        printf("Parachute 1: \n");
+        outcomes[MainFlightPhases::PARA1].print(t_liftoff);
+
+        printf("Parachute 2: \n");
+        outcomes[MainFlightPhases::PARA2].print(t_liftoff);
+
+        printf("Simulation Stopped: \n");
+        outcomes[MainFlightPhases::SIMULATION_STOPPED].print(t_liftoff);
+
+        // auto cpuMeter = Boardcore::CpuMeter::getCpuStats();
+        // printf("max cpu usage: %+.3f\n", cpuMeter.maxValue);
+        // printf("avg cpu usage: %+.3f\n", cpuMeter.mean);
+        // printf("min free heap: %+.3lu\n", cpuMeter.minFreeHeap);
+        // printf("max free stack:%+.3lu\n", cpuMeter.minFreeStack);
+    }
+
+private:
+    void handleEventImpl(const Boardcore::Event& e,
+                         std::vector<MainFlightPhases>& changed_flags) override
+    {
+        switch (e)
+        {
+            case Common::Events::FMM_INIT_ERROR:
+                printf("[HIL] ------- INIT FAILED ! ------- \n");
+            case Common::Events::FMM_INIT_OK:
+                setFlagFlightPhase(MainFlightPhases::CALIBRATION, true);
+                TRACE("[HIL] ------- CALIBRATION ! ------- \n");
+                changed_flags.push_back(MainFlightPhases::CALIBRATION);
+                break;
+            case Common::Events::FLIGHT_DISARMED:
+                setFlagFlightPhase(MainFlightPhases::CALIBRATION_OK, true);
+                TRACE("[HIL] CALIBRATION OK!\n");
+                changed_flags.push_back(MainFlightPhases::CALIBRATION_OK);
+                break;
+            case Common::Events::FLIGHT_ARMED:
+                setFlagFlightPhase(MainFlightPhases::ARMED, true);
+                printf("[HIL] ------- READY TO LAUNCH ! ------- \n");
+                changed_flags.push_back(MainFlightPhases::ARMED);
+                break;
+            case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED:
+                setFlagFlightPhase(MainFlightPhases::LIFTOFF_PIN_DETACHED,
+                                   true);
+                TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n");
+                changed_flags.push_back(MainFlightPhases::LIFTOFF_PIN_DETACHED);
+                break;
+            case Common::Events::FLIGHT_LIFTOFF:
+            case Common::Events::TMTC_FORCE_LAUNCH:
+                t_liftoff = Boardcore::TimestampTimer::getTimestamp();
+                printf("[HIL] ------- LIFTOFF -------: %f, %f \n",
+                       getCurrentPosition().z, getCurrentPosition().vz);
+                changed_flags.push_back(MainFlightPhases::LIFTOFF);
+                break;
+            case Common::Events::ABK_SHADOW_MODE_TIMEOUT:
+                setFlagFlightPhase(MainFlightPhases::AEROBRAKES, true);
+                registerOutcomes(MainFlightPhases::AEROBRAKES);
+                TRACE("[HIL] ABK shadow mode timeout\n");
+                changed_flags.push_back(MainFlightPhases::AEROBRAKES);
+                break;
+            case Common::Events::ADA_SHADOW_MODE_TIMEOUT:
+                TRACE("[HIL] ADA shadow mode timeout\n");
+                break;
+            case Common::Events::ABK_DISABLE:
+                setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
+                TRACE("[HIL] ABK disabled\n");
+                break;
+            case Common::Events::FLIGHT_APOGEE_DETECTED:
+            case Common::Events::CAN_APOGEE_DETECTED:
+                setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false);
+                registerOutcomes(MainFlightPhases::APOGEE);
+                printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n",
+                       getCurrentPosition().z, getCurrentPosition().vz);
+                changed_flags.push_back(MainFlightPhases::APOGEE);
+                break;
+            case Common::Events::FLIGHT_DROGUE_DESCENT:
+            case Common::Events::TMTC_FORCE_EXPULSION:
+                setFlagFlightPhase(MainFlightPhases::PARA1, true);
+                registerOutcomes(MainFlightPhases::PARA1);
+                printf("[HIL] ------- PARA1 ! -------%f, %f \n",
+                       getCurrentPosition().z, getCurrentPosition().vz);
+                changed_flags.push_back(MainFlightPhases::PARA1);
+                break;
+            case Common::Events::FLIGHT_WING_DESCENT:
+            case Common::Events::FLIGHT_DPL_ALT_DETECTED:
+            case Common::Events::TMTC_FORCE_DEPLOYMENT:
+                setFlagFlightPhase(MainFlightPhases::PARA1, false);
+                setFlagFlightPhase(MainFlightPhases::PARA2, true);
+                registerOutcomes(MainFlightPhases::PARA2);
+                printf("[HIL] ------- PARA2 ! ------- %f, %f \n",
+                       getCurrentPosition().z, getCurrentPosition().vz);
+                changed_flags.push_back(MainFlightPhases::PARA2);
+                break;
+            case Common::Events::FLIGHT_LANDING_DETECTED:
+            case Common::Events::TMTC_FORCE_LANDING:
+                t_stop = Boardcore::TimestampTimer::getTimestamp();
+                setFlagFlightPhase(MainFlightPhases::PARA2, false);
+                setFlagFlightPhase(MainFlightPhases::SIMULATION_STOPPED, true);
+                changed_flags.push_back(MainFlightPhases::SIMULATION_STOPPED);
+                registerOutcomes(MainFlightPhases::SIMULATION_STOPPED);
+                TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n",
+                      (double)t_stop / 1000000.0f);
+                printOutcomes();
+                break;
+            default:
+                TRACE("%s event\n", Common::getEventString(e).c_str());
+        }
+    }
+};
+
+}  // namespace HILConfig
\ No newline at end of file
diff --git a/src/tests/hil/Sensors/HILSensors.h b/src/tests/hil/Sensors/HILSensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d642072a419e0428dd72c861ca87a3e461437e1
--- /dev/null
+++ b/src/tests/hil/Sensors/HILSensors.h
@@ -0,0 +1,360 @@
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <hil/HIL.h>
+#include <sensors/HILSensor.h>
+#include <sensors/analog/Pitot/Pitot.h>
+
+#include "../HILSimulationConfig.h"
+#include "Sensors.h"
+
+namespace HILTest
+{
+
+/**
+ * @brief Mock sensor which is used as fake sensor for the combustion chamber
+ * pressure coming from the canbus.
+ */
+class MockChamberSensor : public Boardcore::Sensor<Boardcore::PressureData>
+{
+public:
+    explicit MockChamberSensor() {}
+
+    Boardcore::PressureData sampleImpl() override
+    {
+        return Boardcore::PressureData{0, 0};
+    }
+
+protected:
+    bool init() override { return true; }
+
+    bool selfTest() override { return true; }
+};
+
+class HILSensors : public Sensors
+{
+public:
+    explicit HILSensors(Boardcore::TaskScheduler* sched, Buses* buses,
+                        HILConfig::MainHIL* hil, bool enableHw)
+        : Sensors{sched, buses}, enableHw{enableHw}
+    {
+        using namespace HILConfig;
+        using namespace Boardcore;
+
+        // Creating the fake sensors for the can transmitted samples
+        chamberPressureCreation();
+        pitotCreation(hil);
+
+        hillificator<>(lps28dfw_1, enableHw,
+                       [hil]() { return updateLPS28DFWData(hil); });
+        hillificator<>(lps28dfw_2, enableHw,
+                       [hil]() { return updateLPS28DFWData(hil); });
+        hillificator<>(lps22df, enableHw,
+                       [hil]() { return updateLPS22DFData(hil); });
+        hillificator<>(h3lis331dl, enableHw,
+                       [hil]() { return updateH3LIS331DLData(hil); });
+        hillificator<>(lis2mdl, enableHw,
+                       [hil]() { return updateLIS2MDLData(hil); });
+        hillificator<>(ubxgps, enableHw,
+                       [hil]() { return updateUBXGPSData(hil); });
+        hillificator<>(lsm6dsrx, enableHw,
+                       [hil]() { return updateLSM6DSRXData(hil); });
+        hillificator<>(hscmrnn015pa_1, enableHw,
+                       [hil]() { return updateStaticPressureData(hil); });
+        hillificator<>(hscmrnn015pa_2, enableHw,
+                       [hil]() { return updateStaticPressureData(hil); });
+        hillificator<>(imu, enableHw,
+                       [this, hil]() { return updateIMUData(hil, *this); });
+        hillificator<>(chamber, enableHw,
+                       [hil]() { return updateCCData(hil); });
+    };
+
+    bool start() override
+    {
+        // Registering the fake can sensors
+        if (chamber)
+        {
+            registerSensor(chamber.get(), "chamber",
+                           HILConfig::BARO_CHAMBER_PERIOD,
+                           [this]() { this->chamberPressureCallback(); });
+        }
+
+        if (pitot)
+        {
+            registerSensor(pitot.get(), "Pitot", HILConfig::PITOT_PERIOD,
+                           [this]() { this->pitotCallback(); });
+        }
+
+        return Sensors::start();
+    }
+
+private:
+    void chamberPressureCreation()
+    {
+        chamber = std::make_unique<MockChamberSensor>();
+    }
+
+    void chamberPressureCallback()
+    {
+        // Warning! Here we must use sensor->getLastSample() instead of
+        // getSensorLastSample() since we have to update the "can received
+        // value"
+        auto lastSample = chamber->getLastSample();
+        Boardcore::Logger::getInstance().log(lastSample);
+    }
+
+    void pitotCreation(HILConfig::MainHIL* hil)
+    {
+        pitot = std::make_unique<Boardcore::Pitot>(
+            [hil]() { return getTotalPressurePitot(hil); },
+            [hil]() { return getStaticPressurePitot(hil); });
+        pitot->setReferenceValues(Boardcore::ReferenceValues());
+    }
+
+    void pitotCallback()
+    {
+        // Warning! Here we must use sensor->getLastSample() instead of
+        // getSensorLastSample() since we have to update the "can received
+        // value"
+        auto lastSample = pitot->getLastSample();
+        Boardcore::Logger::getInstance().log(lastSample);
+        setPitot(lastSample);
+    }
+
+    static int getSampleCounter(int nData)
+    {
+        auto ts           = miosix::getTime();
+        auto tsSensorData = Boardcore::ModuleManager::getInstance()
+                                .get<HILConfig::MainHIL>()
+                                ->getTimestampSimulatorData();
+        auto simulationPeriod = Boardcore::ModuleManager::getInstance()
+                                    .get<HILConfig::MainHIL>()
+                                    ->getSimulationPeriod();
+
+        assert(ts >= tsSensorData &&
+               "Actual timestamp is lesser then the packet timestamp");
+
+        if (ts >= tsSensorData + simulationPeriod)
+        {
+            // TODO: Register this as an error
+            return nData - 1;  // Return the last valid index
+        }
+
+        // Getting the index floored
+        int sampleCounter = (ts - tsSensorData) * nData / simulationPeriod;
+
+        if (sampleCounter < 0)
+        {
+            printf("sampleCounter: %d\n", sampleCounter);
+            assert(sampleCounter < 0 && "Calculated a negative index");
+            return 0;
+        }
+
+        return sampleCounter;
+    }
+
+    static Boardcore::LPS28DFWData updateLPS28DFWData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::LPS28DFWData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
+        int iTemp = getSampleCounter(sensorData->temperature.NDATA);
+
+        data.pressureTimestamp = data.temperatureTimestamp = miosix::getTime();
+        data.pressure    = sensorData->barometer1.measures[iBaro];
+        data.temperature = sensorData->temperature.measures[iTemp];
+
+        return data;
+    };
+
+    static Boardcore::LPS22DFData updateLPS22DFData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::LPS22DFData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
+        int iTemp = getSampleCounter(sensorData->temperature.NDATA);
+
+        data.pressureTimestamp = data.temperatureTimestamp = miosix::getTime();
+        data.pressure    = sensorData->barometer1.measures[iBaro];
+        data.temperature = sensorData->temperature.measures[iTemp];
+
+        return data;
+    };
+
+    static Boardcore::H3LIS331DLData updateH3LIS331DLData(
+        HILConfig::MainHIL* hil)
+    {
+        Boardcore::H3LIS331DLData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iAcc = getSampleCounter(sensorData->accelerometer.NDATA);
+
+        data.accelerationTimestamp = miosix::getTime();
+        data.accelerationX = sensorData->accelerometer.measures[iAcc][0];
+        data.accelerationY = sensorData->accelerometer.measures[iAcc][1];
+        data.accelerationZ = sensorData->accelerometer.measures[iAcc][2];
+
+        return data;
+    };
+
+    static Boardcore::LIS2MDLData updateLIS2MDLData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::LIS2MDLData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iMag = getSampleCounter(sensorData->magnetometer.NDATA);
+
+        data.magneticFieldTimestamp = miosix::getTime();
+        data.magneticFieldX = sensorData->magnetometer.measures[iMag][0];
+        data.magneticFieldY = sensorData->magnetometer.measures[iMag][1];
+        data.magneticFieldZ = sensorData->magnetometer.measures[iMag][2];
+
+        return data;
+    };
+
+    static Boardcore::UBXGPSData updateUBXGPSData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::UBXGPSData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iGps = getSampleCounter(sensorData->gps.NDATA);
+
+        data.gpsTimestamp = miosix::getTime();
+
+        data.latitude  = sensorData->gps.positionMeasures[iGps][0];
+        data.longitude = sensorData->gps.positionMeasures[iGps][1];
+        data.height    = sensorData->gps.positionMeasures[iGps][2];
+
+        data.velocityNorth = sensorData->gps.velocityMeasures[iGps][0];
+        data.velocityEast  = sensorData->gps.velocityMeasures[iGps][1];
+        data.velocityDown  = sensorData->gps.velocityMeasures[iGps][2];
+        data.speed         = sqrtf(data.velocityNorth * data.velocityNorth +
+                                   data.velocityEast * data.velocityEast +
+                                   data.velocityDown * data.velocityDown);
+        data.positionDOP   = 0;
+
+        data.fix        = static_cast<uint8_t>(sensorData->gps.fix);
+        data.satellites = static_cast<uint8_t>(sensorData->gps.num_satellites);
+
+        return data;
+    };
+
+    static Boardcore::LSM6DSRXData updateLSM6DSRXData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::LSM6DSRXData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iAcc  = getSampleCounter(sensorData->accelerometer.NDATA);
+        int iGyro = getSampleCounter(sensorData->gyro.NDATA);
+
+        data.accelerationTimestamp = data.angularSpeedTimestamp =
+            miosix::getTime();
+
+        data.accelerationX = sensorData->accelerometer.measures[iAcc][0];
+        data.accelerationY = sensorData->accelerometer.measures[iAcc][1];
+        data.accelerationZ = sensorData->accelerometer.measures[iAcc][2];
+
+        data.angularSpeedX = sensorData->gyro.measures[iGyro][0];
+        data.angularSpeedY = sensorData->gyro.measures[iGyro][1];
+        data.angularSpeedZ = sensorData->gyro.measures[iGyro][2];
+
+        return data;
+    };
+
+    static Boardcore::HSCMRNN015PAData updateStaticPressureData(
+        HILConfig::MainHIL* hil)
+    {
+        Boardcore::HSCMRNN015PAData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
+
+        data.pressureTimestamp = miosix::getTime();
+        data.pressure          = sensorData->barometer1.measures[iBaro];
+
+        return data;
+    };
+
+    static Boardcore::IMUData updateIMUData(HILConfig::MainHIL* hil,
+                                            Sensors& sensors)
+    {
+        return Boardcore::IMUData{
+            sensors.getLSM6DSRXLastSample(), sensors.getLSM6DSRXLastSample(),
+            sensors.getCalibratedMagnetometerLastSample()};
+    };
+
+    static Boardcore::PressureData updateCCData(HILConfig::MainHIL* hil)
+    {
+        Boardcore::PressureData data;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iCC = getSampleCounter(sensorData->pressureChamber.NDATA);
+
+        data.pressureTimestamp = miosix::getTime();
+        data.pressure          = sensorData->pressureChamber.measures[iCC];
+
+        return data;
+    };
+
+    static float getTotalPressurePitot(HILConfig::MainHIL* hil)
+    {
+        float totalPressure;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iPitot = getSampleCounter(sensorData->pitot.NDATA);
+
+        totalPressure = sensorData->pitot.staticPressure[iPitot] +
+                        sensorData->pitot.deltaP[iPitot];
+
+        return totalPressure;
+    };
+
+    static float getStaticPressurePitot(HILConfig::MainHIL* hil)
+    {
+        float staticPressure;
+
+        auto* sensorData = hil->getSensorData();
+
+        int iPitot = getSampleCounter(sensorData->pitot.NDATA);
+
+        staticPressure = sensorData->pitot.staticPressure[iPitot];
+
+        return staticPressure;
+    };
+
+    std::unique_ptr<MockChamberSensor> chamber;
+    std::unique_ptr<Boardcore::Pitot> pitot;
+    bool enableHw;
+};
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/Sensors/Sensors.cpp b/src/tests/hil/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1877bd5c7a98925a66592f57bc34d9005beeffaa
--- /dev/null
+++ b/src/tests/hil/Sensors/Sensors.cpp
@@ -0,0 +1,725 @@
+/* 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 <interfaces-impl/hwmapping.h>
+
+#include "../Buses.h"
+#include "SensorsConfig.h"
+
+namespace HILTest
+{
+using namespace Boardcore;
+using namespace std;
+using namespace SensorsConfig;
+
+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{};
+}
+
+IMUData Sensors::getIMULastSample()
+{
+    miosix::PauseKernelLock lock;
+    return imu != nullptr ? imu->getLastSample() : IMUData{};
+}
+
+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, Buses* buses)
+    : scheduler(sched), buses(buses)
+{
+    // Init all the sensors
+    lps22dfCreation();
+    lps28dfw_1Creation();
+    // lps28dfw_2Creation();
+    h3lis331dlCreation();
+    lis2mdlCreation();
+    ubxgpsCreation();
+    lsm6dsrxCreation();
+    ads131m08Creation();
+    deploymentPressureCreation();
+    staticPressure1Creation();
+    staticPressure2Creation();
+    imuCreation();
+}
+
+bool Sensors::start()
+{
+    // Read the magnetometer calibration from predefined file
+    magCalibration.fromFile("/sd/magCalibration.csv");
+
+    if (lps22df)
+    {
+        registerSensor(lps22df.get(), "LPS22DF", LPS22DF_PERIOD,
+                       [this]() { this->lps22dfCallback(); });
+        addInfoGetter(lps22df.get());
+    }
+
+    if (lps28dfw_1)
+    {
+        registerSensor(lps28dfw_1.get(), "LPS28DFW_1", LPS28DFW_PERIOD,
+                       [this]() { this->lps28dfw_1Callback(); });
+        addInfoGetter(lps28dfw_1.get());
+    }
+
+    if (lps28dfw_2)
+    {
+        registerSensor(lps28dfw_2.get(), "LPS28DFW_2", LPS28DFW_PERIOD,
+                       [this]() { this->lps28dfw_2Callback(); });
+        addInfoGetter(lps28dfw_2.get());
+    }
+
+    if (h3lis331dl)
+    {
+        registerSensor(h3lis331dl.get(), "h3lis331dl", H3LIS331DL_PERIOD,
+                       [this]() { this->h3lis331dlCallback(); });
+        addInfoGetter(h3lis331dl.get());
+    }
+
+    if (lis2mdl)
+    {
+        registerSensor(lis2mdl.get(), "lis2mdl", LIS2MDL_PERIOD,
+                       [this]() { this->lis2mdlCallback(); });
+        addInfoGetter(lis2mdl.get());
+    }
+
+    if (ubxgps)
+    {
+        registerSensor(ubxgps.get(), "ubxgps", UBXGPS_PERIOD,
+                       [this]() { this->ubxgpsCallback(); });
+        addInfoGetter(ubxgps.get());
+    }
+
+    if (lsm6dsrx)
+    {
+        registerSensor(lsm6dsrx.get(), "lsm6dsrx", LSM6DSRX_PERIOD,
+                       [this]() { this->lsm6dsrxCallback(); });
+        addInfoGetter(lsm6dsrx.get());
+    }
+
+    if (ads131m08)
+    {
+        registerSensor(ads131m08.get(), "ads131m08", ADS131M08_PERIOD,
+                       [this]() { this->ads131m08Callback(); });
+        addInfoGetter(ads131m08.get());
+    }
+
+    if (mpxh6400a)
+    {
+        registerSensor(mpxh6400a.get(), "mpxh6400a", ADS131M08_PERIOD,
+                       [this]() { this->deploymentPressureCallback(); });
+    }
+
+    if (hscmrnn015pa_1)
+    {
+        registerSensor(hscmrnn015pa_1.get(), "hscmrnn015pa_1", ADS131M08_PERIOD,
+                       [this]() { this->staticPressure1Callback(); });
+    }
+
+    if (hscmrnn015pa_2)
+    {
+        registerSensor(hscmrnn015pa_2.get(), "hscmrnn015pa_2", ADS131M08_PERIOD,
+                       [this]() { this->staticPressure2Callback(); });
+    }
+
+    if (imu)
+    {
+        registerSensor(imu.get(), "RotatedIMU", IMU_PERIOD,
+                       [this]() { this->imuCallback(); });
+    }
+
+    // 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 = std::make_unique<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::lps22dfCreation()
+{
+
+    // 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 = std::make_unique<LPS22DF>(buses->spi3,
+                                        miosix::sensors::LPS22DF::cs::getPin(),
+                                        config, sensorConfig);
+}
+void Sensors::lps28dfw_1Creation()
+{
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_1 = std::make_unique<LPS28DFW>(buses->i2c1, config);
+}
+void Sensors::lps28dfw_2Creation()
+{
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_2 = std::make_unique<LPS28DFW>(buses->i2c1, config);
+}
+void Sensors::h3lis331dlCreation()
+{
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Create sensor instance with configured parameters
+    h3lis331dl = std::make_unique<H3LIS331DL>(
+        buses->spi3, miosix::sensors::H3LIS331DL::cs::getPin(), config,
+        H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR);
+}
+void Sensors::lis2mdlCreation()
+{
+
+    // 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 = std::make_unique<LIS2MDL>(buses->spi3,
+                                        miosix::sensors::LIS2MDL::cs::getPin(),
+                                        config, sensorConfig);
+}
+
+void Sensors::ubxgpsCreation()
+{
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_64;
+
+    // Create sensor instance with configured parameters
+    ubxgps = std::make_unique<UBXGPSSpi>(
+        buses->spi4, miosix::sensors::GPS::cs::getPin(), config, 5);
+}
+
+void Sensors::lsm6dsrxCreation()
+{
+
+    // 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 = std::make_unique<LSM6DSRX>(
+        buses->spi1, miosix::sensors::LSM6DSRX::cs::getPin(), config,
+        sensorConfig);
+}
+
+void Sensors::ads131m08Creation()
+{
+
+    // 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 = std::make_unique<ADS131M08>(
+        buses->spi4, miosix::sensors::ADS131::cs::getPin(), config,
+        sensorConfig);
+}
+
+void Sensors::deploymentPressureCreation()
+{
+    // 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 = std::make_unique<MPXH6400A>(getVoltage, ADC_VOLTAGE_RANGE);
+}
+
+void Sensors::staticPressure1Creation()
+{
+    // 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 =
+        std::make_unique<HSCMRNN015PA>(getVoltage, ADC_VOLTAGE_RANGE);
+}
+
+void Sensors::staticPressure2Creation()
+{
+    // 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 =
+        std::make_unique<HSCMRNN015PA>(getVoltage, ADC_VOLTAGE_RANGE);
+}
+
+void Sensors::imuCreation()
+{
+    // 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 = std::make_unique<RotatedIMU>(
+        [this]()
+        {
+            auto imu6 = getLSM6DSRXLastSample();
+            auto mag  = getLIS2MDLLastSample();
+
+            return IMUData{imu6, imu6, mag};
+        });
+
+    // 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));
+}
+
+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()
+{
+    IMUData lastSample = imu->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/Sensors/Sensors.h b/src/tests/hil/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..b487b7cd6ec3b49b648fc7fad2b2500fbbf53181
--- /dev/null
+++ b/src/tests/hil/Sensors/Sensors.h
@@ -0,0 +1,232 @@
+/* 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/RotatedIMU/RotatedIMU.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>
+
+#include "../Buses.h"
+#include "SensorsConfig.h"
+#include "SensorsData.h"
+
+namespace HILTest
+{
+class Sensors : public Boardcore::Module
+{
+public:
+    explicit Sensors(Boardcore::TaskScheduler* sched, Buses* buses);
+
+    [[nodiscard]] virtual bool start();
+
+    /**
+     * @brief Stops the sensor manager
+     * @warning Stops the passed scheduler
+     */
+    virtual void stop();
+
+    /**
+     * @brief Returns if all the sensors are started successfully
+     */
+    virtual bool isStarted();
+
+    /**
+     * @brief Calibrates the sensors with an offset
+     */
+    virtual void calibrate();
+
+    /**
+     * @brief Takes the result of the live magnetometer calibration and applies
+     * it to the current calibration + writes it in the csv file
+     */
+    virtual bool writeMagCalibration();
+
+    // Sensor getters
+    virtual Boardcore::LPS22DFData getLPS22DFLastSample();
+    virtual Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
+    virtual Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
+    virtual Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+    virtual Boardcore::LIS2MDLData getLIS2MDLLastSample();
+    virtual Boardcore::UBXGPSData getGPSLastSample();
+    virtual Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+    virtual Boardcore::ADS131M08Data getADS131M08LastSample();
+
+    // Processed getters
+    virtual Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+    virtual Boardcore::BatteryVoltageSensorData
+    getCamBatteryVoltageLastSample();
+    virtual Boardcore::CurrentData getCurrentLastSample();
+    virtual Boardcore::MPXH6400AData getDeploymentPressureLastSample();
+    virtual Boardcore::HSCMRNN015PAData getStaticPressure1LastSample();
+    virtual Boardcore::HSCMRNN015PAData getStaticPressure2LastSample();
+    virtual Boardcore::IMUData getIMULastSample();
+    virtual Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
+
+    // CAN fake sensors setters
+    virtual void setPitot(Boardcore::PitotData data);
+    virtual void setCCPressure(Boardcore::PressureData data);
+    virtual void setBottomTankPressure(Boardcore::PressureData data);
+    virtual void setTopTankPressure(Boardcore::PressureData data);
+    virtual void setTankTemperature(Boardcore::TemperatureData data);
+    virtual void setMotorBatteryVoltage(
+        Boardcore::BatteryVoltageSensorData data);
+    virtual void setMotorCurrent(Boardcore::CurrentData data);
+
+    // CAN fake sensors getters
+    virtual Boardcore::PitotData getPitotLastSample();
+    virtual Boardcore::PressureData getCCPressureLastSample();
+    virtual Boardcore::PressureData getBottomTankPressureLastSample();
+    virtual Boardcore::PressureData getTopTankPressureLastSample();
+    virtual Boardcore::TemperatureData getTankTemperatureLastSample();
+    virtual Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage();
+    virtual Boardcore::CurrentData getMotorCurrent();
+
+    // Returns the sensors statuses
+    std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+    getSensorInfo();
+
+protected:
+    /**
+     * @brief Method to put a sensor in the sensorMap with the relative infos
+     */
+    template <typename T>
+    void registerSensor(Boardcore::Sensor<T>* sensor, const std::string& name,
+                        uint32_t period, std::function<void(void)> callback)
+    {
+        // Emplace the sensor inside the map
+        Boardcore::SensorInfo info(name, period, callback);
+        sensorMap.emplace(std::make_pair(sensor, info));
+    }
+
+    /**
+     * @brief Insert a sensor in the infoGetter.
+     */
+    template <typename T>
+    void addInfoGetter(Boardcore::Sensor<T>* sensor)
+    {
+        // Add the sensor info getter to the array
+        sensorsInit[sensorsId++] = [&]() -> Boardcore::SensorInfo
+        { return manager->getSensorInfo(sensor); };
+    }
+
+    // Creation and callbacks methods
+    void lps22dfCreation();
+    virtual void lps22dfCallback();
+
+    void lps28dfw_1Creation();
+    virtual void lps28dfw_1Callback();
+
+    void lps28dfw_2Creation();
+    virtual void lps28dfw_2Callback();
+
+    void h3lis331dlCreation();
+    virtual void h3lis331dlCallback();
+
+    void lis2mdlCreation();
+    virtual void lis2mdlCallback();
+
+    void ubxgpsCreation();
+    virtual void ubxgpsCallback();
+
+    void lsm6dsrxCreation();
+    virtual void lsm6dsrxCallback();
+
+    void ads131m08Creation();
+    virtual void ads131m08Callback();
+
+    void deploymentPressureCreation();
+    virtual void deploymentPressureCallback();
+
+    void staticPressure1Creation();
+    virtual void staticPressure1Callback();
+
+    void staticPressure2Creation();
+    virtual void staticPressure2Callback();
+
+    void imuCreation();
+    virtual void imuCallback();
+
+    // 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{};
+
+    // Magnetometer live calibration
+    Boardcore::SoftAndHardIronCalibration magCalibrator;
+    Boardcore::SixParametersCorrector magCalibration;
+    miosix::FastMutex calibrationMutex;
+
+    // Fake processed sensors
+    std::unique_ptr<Boardcore::RotatedIMU> imu;
+
+    // Sensor manager
+    std::unique_ptr<Boardcore::SensorManager> manager;
+    Boardcore::SensorManager::SensorMap_t sensorMap;
+    Boardcore::TaskScheduler* scheduler = nullptr;
+    Buses* buses                        = 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");
+
+    // Sensors instances
+    std::unique_ptr<Boardcore::LPS22DF> lps22df;
+    std::unique_ptr<Boardcore::LPS28DFW> lps28dfw_1;
+    std::unique_ptr<Boardcore::LPS28DFW> lps28dfw_2;
+    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;
+
+    // Fake processed sensors
+    std::unique_ptr<Boardcore::MPXH6400A> mpxh6400a;
+    std::unique_ptr<Boardcore::HSCMRNN015PA> hscmrnn015pa_1;
+    std::unique_ptr<Boardcore::HSCMRNN015PA> hscmrnn015pa_2;
+};
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/Sensors/SensorsConfig.h b/src/tests/hil/Sensors/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..09c6cb3e2d007b7be48759c50f42b2ba87ebe7dd
--- /dev/null
+++ b/src/tests/hil/Sensors/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 HILTest
+{
+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 = 10;
+
+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        = 10;  // [ms] 100Hz
+constexpr uint32_t ADS131M08_PERIOD       = 20;  // [ms] 50Hz
+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 HILTest
\ No newline at end of file
diff --git a/src/tests/hil/Sensors/SensorsData.h b/src/tests/hil/Sensors/SensorsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..8bc5ca1efeebc1f1ae014c502f953a3db6370c50
--- /dev/null
+++ b/src/tests/hil/Sensors/SensorsData.h
@@ -0,0 +1,140 @@
+/* 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/LPS28DFW/LPS28DFWData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN015PAData.h>
+
+namespace HILTest
+{
+struct SensorsCalibrationParameter
+{
+    uint64_t timestamp;
+    float referencePressure;
+    float offsetStatic1;
+    float offsetStatic2;
+    float offsetDeployment;
+
+    SensorsCalibrationParameter(uint64_t timestamp, float referencePressure,
+                                float offsetStatic1, float offsetStatic2,
+                                float offsetDeployment)
+        : timestamp(timestamp), referencePressure(referencePressure),
+          offsetStatic1(offsetStatic1), offsetStatic2(offsetStatic2),
+          offsetDeployment(offsetDeployment)
+    {
+    }
+
+    SensorsCalibrationParameter() : SensorsCalibrationParameter(0, 0, 0, 0, 0)
+    {
+    }
+
+    static std::string header()
+    {
+        return "timestamp,referencePressure,offsetStatic1,offsetStatic2,"
+               "offsetDeployment\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << referencePressure << "," << offsetStatic1
+           << "," << offsetStatic2 << "," << offsetDeployment << "\n";
+    }
+};
+struct LPS28DFW_1Data : Boardcore::LPS28DFWData
+{
+    explicit LPS28DFW_1Data(const Boardcore::LPS28DFWData& data)
+        : Boardcore::LPS28DFWData(data)
+    {
+    }
+
+    LPS28DFW_1Data() {}
+
+    static std::string header()
+    {
+        return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << temperatureTimestamp << "," << temperature << "\n";
+    }
+};
+
+struct LPS28DFW_2Data : Boardcore::LPS28DFWData
+{
+    explicit LPS28DFW_2Data(const Boardcore::LPS28DFWData& data)
+        : Boardcore::LPS28DFWData(data)
+    {
+    }
+
+    LPS28DFW_2Data() {}
+
+    static std::string header()
+    {
+        return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << temperatureTimestamp << "," << temperature << "\n";
+    }
+};
+
+struct HSCMRNN015PA_1Data : Boardcore::HSCMRNN015PAData
+{
+    explicit HSCMRNN015PA_1Data(const Boardcore::HSCMRNN015PAData& data)
+        : Boardcore::HSCMRNN015PAData(data)
+    {
+    }
+
+    HSCMRNN015PA_1Data() {}
+
+    static std::string header() { return "pressureTimestamp,pressure\n "; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << "\n";
+    }
+};
+
+struct HSCMRNN015PA_2Data : Boardcore::HSCMRNN015PAData
+{
+    explicit HSCMRNN015PA_2Data(const Boardcore::HSCMRNN015PAData& data)
+        : Boardcore::HSCMRNN015PAData(data)
+    {
+    }
+
+    HSCMRNN015PA_2Data() {}
+
+    static std::string header() { return "pressureTimestamp,pressure\n "; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << "\n";
+    }
+};
+
+}  // namespace HILTest
\ No newline at end of file
diff --git a/src/tests/hil/common/Events.h b/src/tests/hil/common/Events.h
new file mode 100644
index 0000000000000000000000000000000000000000..69ef7d659a5120cc5e60ae6da39dbb833e43bcb4
--- /dev/null
+++ b/src/tests/hil/common/Events.h
@@ -0,0 +1,295 @@
+/* Copyright (c) 2018-2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <events/Event.h>
+
+#include <iostream>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "Topics.h"
+
+namespace Common
+{
+
+enum Events : uint8_t
+{
+    ABK_DISABLE = Boardcore::EV_FIRST_CUSTOM,
+    ABK_OPEN,
+    ABK_RESET,
+    ABK_SHADOW_MODE_TIMEOUT,
+    ABK_WIGGLE,
+    ADA_CALIBRATE,
+    ADA_PRESS_STAB_TIMEOUT,
+    ADA_READY,
+    ADA_FORCE_START,
+    ADA_FORCE_STOP,
+    ADA_SHADOW_MODE_TIMEOUT,
+    ADA_APOGEE_DETECTED,
+    MEA_SHUTDOWN_DETECTED,
+    DPL_CUT_DROGUE,
+    DPL_CUT_TIMEOUT,
+    DPL_NC_OPEN,
+    DPL_NC_RESET,
+    DPL_SERVO_ACTUATION_DETECTED,
+    DPL_WIGGLE,
+    DPL_WES_CAL_DONE,
+    CAN_FORCE_INIT,
+    CAN_ARM,
+    CAN_ENTER_TEST_MODE,
+    CAN_EXIT_TEST_MODE,
+    CAN_CALIBRATE,
+    CAN_DISARM,
+    CAN_LIFTOFF,
+    CAN_APOGEE_DETECTED,
+    CAN_IGNITION,
+    FLIGHT_APOGEE_DETECTED,
+    FLIGHT_ARMED,
+    FLIGHT_DROGUE_DESCENT,
+    FLIGHT_DISARMED,
+    FLIGHT_DPL_ALT_DETECTED,
+    FLIGHT_ERROR_DETECTED,
+    FLIGHT_LANDING_DETECTED,
+    FLIGHT_LANDING_TIMEOUT,
+    FLIGHT_LAUNCH_PIN_DETACHED,
+    FLIGHT_LIFTOFF,
+    FLIGHT_MOTOR_SHUTDOWN,
+    FLIGHT_MISSION_TIMEOUT,
+    FLIGHT_NC_DETACHED,
+    FLIGHT_WING_DESCENT,
+    FMM_ASCENDING,
+    FMM_ALGOS_CAL_DONE,
+    FMM_STOP_LOGGING,
+    FMM_INIT_OK,
+    FMM_INIT_ERROR,
+    FMM_CALIBRATE,
+    FMM_ALGOS_CALIBRATE,
+    FMM_SENSORS_CAL_DONE,
+    FMM_READY,
+    FSR_STATS_TIMEOUT,
+    NAS_CALIBRATE,
+    NAS_READY,
+    NAS_FORCE_START,
+    NAS_FORCE_STOP,
+    TMTC_ARM,
+    TMTC_DISARM,
+    TMTC_CALIBRATE,
+    TMTC_FORCE_INIT,
+    TMTC_FORCE_LAUNCH,
+    TMTC_FORCE_LANDING,
+    TMTC_FORCE_APOGEE,
+    TMTC_FORCE_EXPULSION,
+    TMTC_FORCE_DEPLOYMENT,
+    TMTC_START_LOGGING,
+    TMTC_STOP_LOGGING,
+    TMTC_RESET_BOARD,
+    TMTC_ENTER_TEST_MODE,
+    TMTC_EXIT_TEST_MODE,
+    TMTC_START_RECORDING,
+    TMTC_STOP_RECORDING,
+    MOTOR_START_TARS,
+    MOTOR_STOP_TARS,
+    MOTOR_OPEN_VENTING_VALVE,
+    MOTOR_CLOSE_VENTING_VALVE,
+    MOTOR_OPEN_FILLING_VALVE,
+    MOTOR_CLOSE_FILLING_VALVE,
+    MOTOR_OPEN_RELEASE_VALVE,
+    MOTOR_CLOSE_RELEASE_VALVE,
+    MOTOR_DISCONNECT,
+    MOTOR_IGNITION,
+    MOTOR_OPEN_FEED_VALVE,
+    MOTOR_CLOSE_FEED_VALVE,
+    MOTOR_MANUAL_ACTION,
+    MOTOR_OPEN_OXIDANT,
+    MOTOR_SHADOW_MODE_TIMEOUT,
+    TARS_WASHING_DONE,
+    TARS_CHECK_PRESSURE_STABILIZE,
+    TARS_PRESSURE_STABILIZED,
+    TARS_FILLING_DONE,
+    TARS_REFINING_DONE,
+    ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+    LAST_EVENT
+};
+
+inline std::string getEventString(uint8_t event)
+{
+    static const std::map<uint8_t, std::string> event_string_map{
+        {ABK_DISABLE, "ABK_DISABLE"},
+        {ABK_OPEN, "ABK_OPEN"},
+        {ABK_RESET, "ABK_RESET"},
+        {ABK_SHADOW_MODE_TIMEOUT, "ABK_SHADOW_MODE_TIMEOUT"},
+        {ABK_WIGGLE, "ABK_WIGGLE"},
+        {ADA_CALIBRATE, "ADA_CALIBRATE"},
+        {ADA_PRESS_STAB_TIMEOUT, "ADA_PRESS_STAB_TIMEOUT"},
+        {ADA_READY, "ADA_READY"},
+        {ADA_FORCE_START, "ADA_FORCE_START"},
+        {ADA_FORCE_STOP, "ADA_FORCE_STOP"},
+        {ADA_SHADOW_MODE_TIMEOUT, "ADA_SHADOW_MODE_TIMEOUT"},
+        {ADA_APOGEE_DETECTED, "ADA_APOGEE_DETECTED"},
+        {MEA_SHUTDOWN_DETECTED, "MEA_SHUTDOWN_DETECTED"},
+        {DPL_CUT_DROGUE, "DPL_CUT_DROGUE"},
+        {DPL_CUT_TIMEOUT, "DPL_CUT_TIMEOUT"},
+        {DPL_NC_OPEN, "DPL_NC_OPEN"},
+        {DPL_NC_RESET, "DPL_NC_RESET"},
+        {DPL_SERVO_ACTUATION_DETECTED, "DPL_SERVO_ACTUATION_DETECTED"},
+        {DPL_WIGGLE, "DPL_WIGGLE"},
+        {DPL_WES_CAL_DONE, "DPL_WES_CAL_DONE"},
+        {CAN_FORCE_INIT, "CAN_FORCE_INIT"},
+        {CAN_ARM, "CAN_ARM"},
+        {CAN_ENTER_TEST_MODE, "CAN_ENTER_TEST_MODE"},
+        {CAN_EXIT_TEST_MODE, "CAN_EXIT_TEST_MODE"},
+        {CAN_CALIBRATE, "CAN_CALIBRATE"},
+        {CAN_DISARM, "CAN_DISARM"},
+        {CAN_LIFTOFF, "CAN_LIFTOFF"},
+        {CAN_APOGEE_DETECTED, "CAN_APOGEE_DETECTED"},
+        {CAN_IGNITION, "CAN_IGNITION"},
+        {FLIGHT_APOGEE_DETECTED, "FLIGHT_APOGEE_DETECTED"},
+        {FLIGHT_ARMED, "FLIGHT_ARMED"},
+        {FLIGHT_DROGUE_DESCENT, "FLIGHT_DROGUE_DESCENT"},
+        {FLIGHT_DISARMED, "FLIGHT_DISARMED"},
+        {FLIGHT_DPL_ALT_DETECTED, "FLIGHT_DPL_ALT_DETECTED"},
+        {FLIGHT_ERROR_DETECTED, "FLIGHT_ERROR_DETECTED"},
+        {FLIGHT_LANDING_DETECTED, "FLIGHT_LANDING_DETECTED"},
+        {FLIGHT_LAUNCH_PIN_DETACHED, "FLIGHT_LAUNCH_PIN_DETACHED"},
+        {FLIGHT_LIFTOFF, "FLIGHT_LIFTOFF"},
+        {FLIGHT_MOTOR_SHUTDOWN, "FLIGHT_MOTOR_SHUTDOWN"},
+        {FLIGHT_LANDING_TIMEOUT, "FLIGHT_LANDING_TIMEOUT"},
+        {FLIGHT_NC_DETACHED, "FLIGHT_NC_DETACHED"},
+        {FLIGHT_MISSION_TIMEOUT, "FLIGHT_MISSION_TIMEOUT"},
+        {FLIGHT_WING_DESCENT, "FLIGHT_WING_DESCENT"},
+        {FMM_ASCENDING, "FMM_ASCENDING"},
+        {FMM_ALGOS_CAL_DONE, "FMM_ALGOS_CAL_DONE"},
+        {FMM_STOP_LOGGING, "FMM_STOP_LOGGING"},
+        {FMM_INIT_OK, "FMM_INIT_OK"},
+        {FMM_INIT_ERROR, "FMM_INIT_ERROR"},
+        {FMM_CALIBRATE, "FMM_CALIBRATE"},
+        {FMM_ALGOS_CALIBRATE, "FMM_ALGOS_CALIBRATE"},
+        {FMM_SENSORS_CAL_DONE, "FMM_SENSORS_CAL_DONE"},
+        {FMM_READY, "FMM_READY"},
+        {FSR_STATS_TIMEOUT, "FSR_STATS_TIMEOUT"},
+        {NAS_CALIBRATE, "NAS_CALIBRATE"},
+        {NAS_FORCE_START, "NAS_FORCE_START"},
+        {NAS_FORCE_STOP, "NAS_FORCE_STOP"},
+        {NAS_READY, "NAS_READY"},
+        {TMTC_ARM, "TMTC_ARM"},
+        {TMTC_DISARM, "TMTC_DISARM"},
+        {TMTC_CALIBRATE, "TMTC_CALIBRATE"},
+        {TMTC_FORCE_INIT, "TMTC_FORCE_INIT"},
+        {TMTC_FORCE_LAUNCH, "TMTC_FORCE_LAUNCH"},
+        {TMTC_FORCE_LANDING, "TMTC_FORCE_LANDING"},
+        {TMTC_FORCE_APOGEE, "TMTC_FORCE_APOGEE"},
+        {TMTC_FORCE_EXPULSION, "TMTC_FORCE_EXPULSION"},
+        {TMTC_FORCE_DEPLOYMENT, "TMTC_FORCE_DEPLOYMENT"},
+        {TMTC_START_LOGGING, "TMTC_START_LOGGING"},
+        {TMTC_STOP_LOGGING, "TMTC_STOP_LOGGING"},
+        {TMTC_RESET_BOARD, "TMTC_RESET_BOARD"},
+        {TMTC_ENTER_TEST_MODE, "TMTC_ENTER_TEST_MODE"},
+        {TMTC_EXIT_TEST_MODE, "TMTC_EXIT_TEST_MODE"},
+        {TMTC_START_RECORDING, "TMTC_START_RECORDING"},
+        {TMTC_STOP_RECORDING, "TMTC_STOP_RECORDING"},
+        {MOTOR_START_TARS, "MOTOR_START_TARS"},
+        {MOTOR_STOP_TARS, "MOTOR_STOP_TARS"},
+        {MOTOR_OPEN_VENTING_VALVE, "MOTOR_OPEN_VENTING_VALVE"},
+        {MOTOR_CLOSE_VENTING_VALVE, "MOTOR_CLOSE_VENTING_VALVE"},
+        {MOTOR_OPEN_FILLING_VALVE, "MOTOR_OPEN_FILLING_VALVE"},
+        {MOTOR_CLOSE_FILLING_VALVE, "MOTOR_CLOSE_FILLING_VALVE"},
+        {MOTOR_OPEN_RELEASE_VALVE, "MOTOR_OPEN_RELEASE_VALVE"},
+        {MOTOR_CLOSE_RELEASE_VALVE, "MOTOR_CLOSE_RELEASE_VALVE"},
+        {MOTOR_DISCONNECT, "MOTOR_DISCONNECT"},
+        {MOTOR_IGNITION, "MOTOR_IGNITION"},
+        {MOTOR_OPEN_FEED_VALVE, "MOTOR_OPEN_FEED_VALVE"},
+        {MOTOR_CLOSE_FEED_VALVE, "MOTOR_CLOSE_FEED_VALVE"},
+        {MOTOR_MANUAL_ACTION, "MOTOR_MANUAL_ACTION"},
+        {MOTOR_OPEN_OXIDANT, "MOTOR_OPEN_OXIDANT"},
+        {MOTOR_SHADOW_MODE_TIMEOUT, "MOTOR_SHADOW_MODE_TIMEOUT"},
+        {TARS_WASHING_DONE, "TARS_WASHING_DONE"},
+        {TARS_CHECK_PRESSURE_STABILIZE, "TARS_CHECK_PRESSURE_STABILIZE"},
+        {TARS_PRESSURE_STABILIZED, "TARS_PRESSURE_STABILIZED"},
+        {TARS_FILLING_DONE, "TARS_FILLING_DONE"},
+        {TARS_REFINING_DONE, "TARS_REFINING_DONE"},
+        {ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+         "ALTITUDE_TRIGGER_ALTITUDE_REACHED"},
+        {LAST_EVENT, "LAST_EVENT"},
+    };
+
+    auto it = event_string_map.find(event);
+    return it == event_string_map.end() ? "EV_UNKNOWN" : it->second;
+}
+
+struct LiftoffEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+struct ApogeeEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+struct NoseconeEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+struct ExpulsionEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+struct MainEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+struct LandingEvent
+{
+    uint64_t timestamp;
+
+    static std::string header() { return "timestamp\n"; }
+
+    void print(std::ostream& os) const { os << timestamp << "\n"; }
+};
+
+}  // namespace Common
diff --git a/src/tests/hil/common/ReferenceConfig.h b/src/tests/hil/common/ReferenceConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..2118c7709b7598d531a5499f1b68813ccaf14920
--- /dev/null
+++ b/src/tests/hil/common/ReferenceConfig.h
@@ -0,0 +1,79 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/ReferenceValues.h>
+
+namespace Common
+{
+
+namespace ReferenceConfig
+{
+
+#if defined(EUROC)
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+    160.0,      // [m] Altitude
+    99418.0,    // [Pa] Pressure
+    288.15,     // [K] Temperature
+    39.389733,  // [deg] Start latitude
+    -8.288992,  // [deg] Start longitude
+    Boardcore::Constants::MSL_PRESSURE,
+    Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.5939, -0.0126, 0.8044);
+
+#elif defined(ROCCARASO)
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+    1414.0,      // [m] Altitude
+    85452.0,     // [Pa] Pressure
+    278.95,      // [K] Temperature
+    41.8089005,  // [deg] Start latitude
+    14.0546716,  // [deg] Start longitude
+    Boardcore::Constants::MSL_PRESSURE,
+    Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.5244, 0.0368, 0.8507);
+
+#else  // Milan
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+    135.0,              // [m] Altitude
+    99714.0,            // [Pa] Pressure
+    278.27,             // [K] Temperature
+    45.50106793771145,  // [deg] Start latitude
+    9.156376900740167,  // [deg] Start longitude
+    Boardcore::Constants::MSL_PRESSURE,
+    Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.4732, 0.0272, 0.8805);
+
+#endif
+
+}  // namespace ReferenceConfig
+
+}  // namespace Common
diff --git a/src/tests/hil/common/Topics.h b/src/tests/hil/common/Topics.h
new file mode 100644
index 0000000000000000000000000000000000000000..eaa0429e89f472c4efe71081ac28124aea898faf
--- /dev/null
+++ b/src/tests/hil/common/Topics.h
@@ -0,0 +1,55 @@
+/* Copyright (c) 2018-2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <cstdint>
+#include <string>
+#include <vector>
+
+namespace Common
+{
+
+enum Topics : uint8_t
+{
+    TOPIC_ABK,
+    TOPIC_ADA,
+    TOPIC_MEA,
+    TOPIC_DPL,
+    TOPIC_CAN,
+    TOPIC_FLIGHT,
+    TOPIC_FMM,
+    TOPIC_FSR,
+    TOPIC_NAS,
+    TOPIC_TMTC,
+    TOPIC_MOTOR,
+    TOPIC_TARS,
+    TOPIC_ALT,
+};
+
+const std::vector<uint8_t> TOPICS_LIST{
+    TOPIC_ABK,    TOPIC_ADA,  TOPIC_MEA, TOPIC_DPL, TOPIC_CAN,
+    TOPIC_FLIGHT, TOPIC_FMM,  TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC,
+    TOPIC_MOTOR,  TOPIC_TARS, TOPIC_ALT,
+};
+
+}  // namespace Common
diff --git a/src/tests/hil/test-hil.cpp b/src/tests/hil/test-hil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b5790f6ce38e6835fdafe7b3fb983ec354e8396
--- /dev/null
+++ b/src/tests/hil/test-hil.cpp
@@ -0,0 +1,124 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <algorithms/AirBrakes/TrajectoryPoint.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 <hil/HIL.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "Buses.h"
+#include "HILSimulationConfig.h"
+#include "Sensors/HILSensors.h"
+#include "Sensors/Sensors.h"
+#include "Sensors/SensorsConfig.h"
+
+using namespace Boardcore;
+using namespace HILConfig;
+using namespace HILTest;
+
+static const bool HIL_TEST = true;
+
+int main()
+{
+    // Overall status, if at some point it becomes false, there is a problem
+    // somewhere
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+    Boardcore::TaskScheduler scheduler;
+
+    // Create modules
+    Buses* buses = new Buses();
+    Sensors* sensors;
+
+    if (HIL_TEST)
+    {
+        // Create hil modules
+        auto* hilPhasesManager = new MainHILPhasesManager(
+            []() { return Boardcore::TimedTrajectoryPoint(); });
+        auto* hilTransceiver =
+            new MainHILTransceiver(buses->usart2, hilPhasesManager);
+
+        // Create HIL class where we specify how to use previous modules to
+        // assemble ActuatorData
+        MainHIL* hil = new MainHIL(
+            hilTransceiver, hilPhasesManager,
+            [&]()
+            {
+                auto actuatorData = ActuatorData();
+                return actuatorData;
+            },
+            SIMULATION_PERIOD);
+
+        if (HIL_TEST)
+        {
+            initResult &= ModuleManager::getInstance().insert<MainHIL>(hil);
+        }
+        sensors = new HILSensors(&scheduler, buses, hil, false);
+    }
+    else
+    {
+        sensors = new Sensors(&scheduler, buses);
+    }
+
+    // Insert modules
+    initResult &= ModuleManager::getInstance().insert<Buses>(buses);
+    initResult &= ModuleManager::getInstance().insert<Sensors>(sensors);
+
+    // Start modules
+    initResult &= EventBroker::getInstance().start();
+    initResult &= scheduler.start();
+
+    if (HIL_TEST)
+    {
+        initResult &= ModuleManager::getInstance().get<MainHIL>()->start();
+
+        ModuleManager::getInstance().get<MainHIL>()->waitStartSimulation();
+    }
+
+    initResult &= ModuleManager::getInstance().get<Sensors>()->start();
+
+    // Check the init result and launch an event
+    printf(initResult ? "Init OK\n" : "Init ERROR\n");
+
+    // Periodic statistics
+    while (true)
+    {
+        Thread::sleep(1000);
+        printf("max: %.3f, min: %.3f, free stack: %ld, free heap: %ld\n",
+               CpuMeter::getCpuStats().maxValue,
+               CpuMeter::getCpuStats().minValue,
+               CpuMeter::getCpuStats().freeStack,
+               CpuMeter::getCpuStats().freeHeap);
+        CpuMeter::resetCpuStats();
+        StackLogger::getInstance().log();
+    }
+
+    return 0;
+}
\ No newline at end of file