From 3d46d7131edeba9c6d679c01e2d314c51bc303bb Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Sat, 24 Aug 2024 18:08:28 +0200
Subject: [PATCH] [Payload][HIL] Initial implementation of HIL simulation
 support

- Introduced support for the HIL framework in the Payload board
- Wait for simulation start before starting sensors
- Introduce a persistent flag to reboot into HIL mode
- Initialize Mavlink over serial only if not in HIL mode
---
 cmake/dependencies.cmake                      |   2 +
 src/boards/Payload/CanHandler/CanHandler.cpp  |   2 +-
 .../Payload/Configs/HILSimulationConfig.h     |  79 +++++
 src/boards/Payload/HIL/HIL.cpp                | 287 ++++++++++++++++++
 src/boards/Payload/HIL/HIL.h                  |  85 ++++++
 src/boards/Payload/HIL/HILData.h              | 259 ++++++++++++++++
 .../Payload/PersistentVars/PersistentVars.cpp |  48 +++
 .../Payload/PersistentVars/PersistentVars.h   |  37 +++
 src/boards/Payload/Radio/MessageHandler.cpp   |   2 +-
 src/boards/Payload/Radio/Radio.cpp            |   6 +-
 src/boards/Payload/Radio/Radio.h              |   1 +
 src/boards/Payload/Sensors/HILSensors.h       | 253 +++++++++++++++
 .../FlightModeManager/FlightModeManager.cpp   |  44 +++
 .../FlightModeManager/FlightModeManager.h     |   1 +
 src/entrypoints/Payload/payload-entry.cpp     |  31 +-
 15 files changed, 1130 insertions(+), 7 deletions(-)
 create mode 100644 src/boards/Payload/Configs/HILSimulationConfig.h
 create mode 100644 src/boards/Payload/HIL/HIL.cpp
 create mode 100644 src/boards/Payload/HIL/HIL.h
 create mode 100644 src/boards/Payload/HIL/HILData.h
 create mode 100644 src/boards/Payload/PersistentVars/PersistentVars.cpp
 create mode 100644 src/boards/Payload/PersistentVars/PersistentVars.h
 create mode 100644 src/boards/Payload/Sensors/HILSensors.h

diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 240c2798b..cf03a37d9 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -82,7 +82,9 @@ set(PAYLOAD_COMPUTER
     src/boards/Payload/Actuators/Actuators.cpp
     src/boards/Payload/CanHandler/CanHandler.cpp
     src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
+    src/boards/Payload/HIL/HIL.cpp
     src/boards/Payload/Sensors/Sensors.cpp
+    src/boards/Payload/PersistentVars/PersistentVars.cpp
     src/boards/Payload/PinHandler/PinHandler.cpp
     src/boards/Payload/Radio/Radio.cpp
     src/boards/Payload/Radio/MessageHandler.cpp
diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp
index 549e05d6e..14eb457bd 100644
--- a/src/boards/Payload/CanHandler/CanHandler.cpp
+++ b/src/boards/Payload/CanHandler/CanHandler.cpp
@@ -115,7 +115,7 @@ bool CanHandler::start()
                   .logNumber = static_cast<int16_t>(logStats.logNumber),
                   .state     = static_cast<uint8_t>(state),
                   .armed     = state == FlightModeManagerState::ARMED,
-                  .hil       = false,  // TODO: hil
+                  .hil       = PersistentVars::getHilMode(),
                   .logGood   = logStats.lastWriteError == 0,
             };
 
diff --git a/src/boards/Payload/Configs/HILSimulationConfig.h b/src/boards/Payload/Configs/HILSimulationConfig.h
new file mode 100644
index 000000000..18ba5b5fa
--- /dev/null
+++ b/src/boards/Payload/Configs/HILSimulationConfig.h
@@ -0,0 +1,79 @@
+/* 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 <units/Frequency.h>
+#include <units/Units.h>
+
+#include <chrono>
+
+#include "SensorsConfig.h"
+
+namespace Payload
+{
+namespace Config
+{
+namespace HIL
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr bool ENABLE_HW = false;
+
+// Period of simulation [ms]
+constexpr auto SIMULATION_RATE = 10_hz;
+
+// Number of samples per sensor at each simulator iteration
+constexpr int N_DATA_ACCEL       = 10;  // #samples
+constexpr int N_DATA_GYRO        = 10;  // #samples
+constexpr int N_DATA_MAGNETO     = 10;  // #samples
+constexpr int N_DATA_GPS         = 1;   // #samples
+constexpr int N_DATA_BARO_STATIC = 10;  // #samples
+constexpr int N_DATA_BARO_PITOT  = 10;  // #samples
+constexpr int N_DATA_TEMP        = 1;   // #samples
+
+// clang-format off
+// Checking if the data coming from simulator is enough
+static_assert(N_DATA_ACCEL * SIMULATION_RATE >= Sensors::LSM6DSRX::SAMPLING_RATE,
+              "N_DATA_ACCEL not enough");
+static_assert(N_DATA_GYRO * SIMULATION_RATE >= Sensors::LSM6DSRX::SAMPLING_RATE,
+              "N_DATA_GYRO not enough");
+static_assert(N_DATA_MAGNETO * SIMULATION_RATE >= Sensors::LIS2MDL::SAMPLING_RATE,
+              "N_DATA_MAGNETO not enough");
+static_assert(N_DATA_GPS * SIMULATION_RATE >= Sensors::UBXGPS::SAMPLING_RATE,
+              "N_DATA_GPS not enough");
+static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::ADS131M08::SAMPLING_RATE,
+              "N_DATA_BARO_STATIC not enough");
+static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS22DF::SAMPLING_RATE,
+              "N_DATA_BARO_STATIC not enough");
+static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS28DFW::SAMPLING_RATE,
+              "N_DATA_BARO_STATIC not enough");
+static_assert(N_DATA_BARO_PITOT * SIMULATION_RATE >= Sensors::StaticPressure::SAMPLING_RATE,
+              "N_DATA_BARO_PITOT not enough");
+static_assert(N_DATA_BARO_PITOT * SIMULATION_RATE >= Sensors::DynamicPressure::SAMPLING_RATE,
+              "N_DATA_BARO_PITOT not enough");
+// clang-format on
+
+}  // namespace HIL
+}  // namespace Config
+}  // namespace Payload
diff --git a/src/boards/Payload/HIL/HIL.cpp b/src/boards/Payload/HIL/HIL.cpp
new file mode 100644
index 000000000..560faeb89
--- /dev/null
+++ b/src/boards/Payload/HIL/HIL.cpp
@@ -0,0 +1,287 @@
+/* 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 "HIL.h"
+
+#include <Payload/Actuators/Actuators.h>
+#include <Payload/Buses.h>
+#include <Payload/Configs/HILSimulationConfig.h>
+#include <Payload/Configs/SensorsConfig.h>
+#include <Payload/HIL/HILData.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+#include <hil/HIL.h>
+#include <interfaces-impl/hwmapping.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "HILData.h"
+
+namespace Payload
+{
+
+PayloadHILPhasesManager::PayloadHILPhasesManager(
+    std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+    : HILPhasesManager<PayloadFlightPhases, SimulatorData, ActuatorData>(
+          getCurrentPosition)
+{
+    flagsFlightPhases = {{PayloadFlightPhases::SIMULATION_STARTED, false},
+                         {PayloadFlightPhases::INITIALIZED, false},
+                         {PayloadFlightPhases::CALIBRATION, false},
+                         {PayloadFlightPhases::CALIBRATION_OK, false},
+                         {PayloadFlightPhases::ARMED, false},
+                         {PayloadFlightPhases::LIFTOFF_PIN_DETACHED, false},
+                         {PayloadFlightPhases::LIFTOFF, false},
+                         {PayloadFlightPhases::MOTOR_SHUTDOWN, false},
+                         {PayloadFlightPhases::AEROBRAKES, false},
+                         {PayloadFlightPhases::APOGEE, false},
+                         {PayloadFlightPhases::PARA1, false},
+                         {PayloadFlightPhases::PARA2, false},
+                         {PayloadFlightPhases::SIMULATION_STOPPED, false}};
+
+    prev_flagsFlightPhases = flagsFlightPhases;
+
+    // Subscribe to all the topics
+    auto& eventBroker = Boardcore::EventBroker::getInstance();
+    eventBroker.subscribe(this, Common::TOPIC_ABK);
+    eventBroker.subscribe(this, Common::TOPIC_ADA);
+    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 PayloadHILPhasesManager::processFlagsImpl(
+    const SimulatorData& simulatorData,
+    std::vector<PayloadFlightPhases>& changed_flags)
+{
+    if (simulatorData.signal ==
+        static_cast<float>(HILSignal::SIMULATION_STARTED))
+    {
+        miosix::reboot();
+    }
+
+    if (simulatorData.signal ==
+        static_cast<float>(HILSignal::SIMULATION_STOPPED))
+    {
+        Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LANDING,
+                                                   Common::TOPIC_TMTC);
+    }
+
+    if (simulatorData.signal ==
+        static_cast<float>(HILSignal::SIMULATION_FORCE_LAUNCH))
+    {
+        Boardcore::EventBroker::getInstance().post(Common::TMTC_ARM,
+                                                   Common::TOPIC_TMTC);
+        Thread::sleep(250);
+        Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LAUNCH,
+                                                   Common::TOPIC_TMTC);
+    }
+
+    // set true when the first packet from the simulator arrives
+    if (isSetTrue(PayloadFlightPhases::SIMULATION_STARTED))
+    {
+        t_start = Boardcore::TimestampTimer::getTimestamp();
+
+        printf("[HIL] ------- SIMULATION STARTED ! ------- \n");
+        changed_flags.push_back(PayloadFlightPhases::SIMULATION_STARTED);
+    }
+}
+
+void PayloadHILPhasesManager::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 shutdown: \n");
+    outcomes[PayloadFlightPhases::MOTOR_SHUTDOWN].print(t_liftoff);
+
+    printf("Apogee: \n");
+    outcomes[PayloadFlightPhases::APOGEE].print(t_liftoff);
+
+    printf("Parachute 1: \n");
+    outcomes[PayloadFlightPhases::PARA1].print(t_liftoff);
+
+    printf("Parachute 2: \n");
+    outcomes[PayloadFlightPhases::PARA2].print(t_liftoff);
+
+    printf("Simulation Stopped: \n");
+    outcomes[PayloadFlightPhases::SIMULATION_STOPPED].print(t_liftoff);
+}
+
+void PayloadHILPhasesManager::handleEventImpl(
+    const Boardcore::Event& e, std::vector<PayloadFlightPhases>& changed_flags)
+{
+    switch (e)
+    {
+        case Common::Events::FMM_INIT_ERROR:
+            printf("[HIL] ------- INIT FAILED ! ------- \n");
+
+        case Common::Events::FMM_INIT_OK:
+            setFlagFlightPhase(PayloadFlightPhases::INITIALIZED, true);
+            TRACE("[HIL] ------- INITIALIZED ! ------- \n");
+            changed_flags.push_back(PayloadFlightPhases::INITIALIZED);
+            break;
+        case Common::Events::FMM_CALIBRATE:
+        case Common::Events::CAN_CALIBRATE:
+        case Common::Events::TMTC_CALIBRATE:
+            setFlagFlightPhase(PayloadFlightPhases::CALIBRATION, true);
+            TRACE("[HIL] ------- CALIBRATION ! ------- \n");
+            changed_flags.push_back(PayloadFlightPhases::CALIBRATION);
+            break;
+        case Common::Events::FLIGHT_DISARMED:
+            setFlagFlightPhase(PayloadFlightPhases::CALIBRATION_OK, true);
+            TRACE("[HIL] ------- CALIBRATION OK ! ------- \n");
+            changed_flags.push_back(PayloadFlightPhases::CALIBRATION_OK);
+            break;
+        case Common::Events::FLIGHT_ARMED:
+            setFlagFlightPhase(PayloadFlightPhases::ARMED, true);
+            printf("[HIL] ------- READY TO LAUNCH ! ------- \n");
+            changed_flags.push_back(PayloadFlightPhases::ARMED);
+            break;
+        case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED:
+            setFlagFlightPhase(PayloadFlightPhases::LIFTOFF_PIN_DETACHED, true);
+            TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n");
+            changed_flags.push_back(PayloadFlightPhases::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(PayloadFlightPhases::LIFTOFF);
+            break;
+        case Common::Events::FLIGHT_MOTOR_SHUTDOWN:
+            setFlagFlightPhase(PayloadFlightPhases::MOTOR_SHUTDOWN, true);
+            registerOutcomes(PayloadFlightPhases::MOTOR_SHUTDOWN);
+            printf("[HIL] ------- MOTOR SHUTDOWN ! ------- %f, %f \n",
+                   getCurrentPosition().z, getCurrentPosition().vz);
+            changed_flags.push_back(PayloadFlightPhases::MOTOR_SHUTDOWN);
+            break;
+        case Common::Events::FLIGHT_APOGEE_DETECTED:
+        case Common::Events::CAN_APOGEE_DETECTED:
+        case Common::Events::TMTC_FORCE_APOGEE:
+            setFlagFlightPhase(PayloadFlightPhases::AEROBRAKES, false);
+            registerOutcomes(PayloadFlightPhases::APOGEE);
+            printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n",
+                   getCurrentPosition().z, getCurrentPosition().vz);
+            changed_flags.push_back(PayloadFlightPhases::APOGEE);
+            break;
+        case Common::Events::FLIGHT_DROGUE_DESCENT:
+        case Common::Events::TMTC_FORCE_EXPULSION:
+            setFlagFlightPhase(PayloadFlightPhases::PARA1, true);
+            registerOutcomes(PayloadFlightPhases::PARA1);
+            printf("[HIL] ------- PARA1 ! -------%f, %f \n",
+                   getCurrentPosition().z, getCurrentPosition().vz);
+            changed_flags.push_back(PayloadFlightPhases::PARA1);
+            break;
+        case Common::Events::FLIGHT_WING_DESCENT:
+        case Common::Events::FLIGHT_DPL_ALT_DETECTED:
+        case Common::Events::TMTC_FORCE_DEPLOYMENT:
+            setFlagFlightPhase(PayloadFlightPhases::PARA1, false);
+            setFlagFlightPhase(PayloadFlightPhases::PARA2, true);
+            registerOutcomes(PayloadFlightPhases::PARA2);
+            printf("[HIL] ------- PARA2 ! ------- %f, %f \n",
+                   getCurrentPosition().z, getCurrentPosition().vz);
+            changed_flags.push_back(PayloadFlightPhases::PARA2);
+            break;
+        case Common::Events::FLIGHT_LANDING_DETECTED:
+        case Common::Events::TMTC_FORCE_LANDING:
+            t_stop = Boardcore::TimestampTimer::getTimestamp();
+            setFlagFlightPhase(PayloadFlightPhases::PARA2, false);
+            setFlagFlightPhase(PayloadFlightPhases::SIMULATION_STOPPED, true);
+            changed_flags.push_back(PayloadFlightPhases::SIMULATION_STOPPED);
+            registerOutcomes(PayloadFlightPhases::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());
+    }
+}
+
+PayloadHIL::PayloadHIL()
+    : Boardcore::HIL<PayloadFlightPhases, SimulatorData, ActuatorData>(
+          nullptr, nullptr, [this]() { return updateActuatorData(); },
+          1000 / Config::HIL::SIMULATION_RATE.value())
+{
+}
+
+bool PayloadHIL::start()
+{
+    auto* nas      = getModule<Payload::NASController>();
+    auto& hilUsart = getModule<Payload::Buses>()->HILUart();
+
+    hilPhasesManager = new PayloadHILPhasesManager(
+        [nas]()
+        { return Boardcore::TimedTrajectoryPoint(nas->getNasState()); });
+
+    hilTransceiver = new PayloadHILTransceiver(hilUsart, hilPhasesManager);
+
+    return Boardcore::HIL<PayloadFlightPhases, SimulatorData,
+                          ActuatorData>::start();
+}
+
+ActuatorData PayloadHIL::updateActuatorData()
+{
+    auto nas       = getModule<Payload::NASController>();
+    auto wes       = getModule<Payload::WindEstimation>();
+    auto fmm       = getModule<Payload::FlightModeManager>();
+    auto wing      = getModule<Payload::WingController>();
+    auto actuators = getModule<Payload::Actuators>();
+
+    NASStateHIL nasStateHIL(nas->getNasState());
+
+    ActuatorsStateHIL actuatorsStateHIL(
+        actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO),
+        actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO),
+        static_cast<float>(miosix::gpios::mainDeploy::value()));
+
+    WESDataHIL wesDataHIL(wes->getWindEstimationScheme());
+
+    auto deltaA = actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO) -
+                  actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO);
+
+    Eigen::Vector2f heading;
+
+    auto psiRef = wing->calculateTargetAngle(
+        {nasStateHIL.n, nasStateHIL.e, nasStateHIL.d}, heading);
+
+    GuidanceDataHIL guidanceData(psiRef, deltaA, heading[0], heading[1]);
+
+    // Returning the feedback for the simulator
+    return ActuatorData(
+        nasStateHIL, actuatorsStateHIL, wesDataHIL, guidanceData,
+        (fmm->testState(&Payload::FlightModeManager::FlyingAscending) ? 3 : 0));
+};
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/HIL/HIL.h b/src/boards/Payload/HIL/HIL.h
new file mode 100644
index 000000000..72bd8b5a2
--- /dev/null
+++ b/src/boards/Payload/HIL/HIL.h
@@ -0,0 +1,85 @@
+/* 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 <Payload/Actuators/Actuators.h>
+#include <Payload/Buses.h>
+#include <Payload/Configs/HILSimulationConfig.h>
+#include <Payload/HIL/HILData.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <Payload/WindEstimationScheme/WindEstimation.h>
+#include <common/Events.h>
+#include <hil/HIL.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Payload
+{
+
+class PayloadHILTransceiver
+    : public Boardcore::HILTransceiver<PayloadFlightPhases, SimulatorData,
+                                       ActuatorData>
+{
+    using Boardcore::HILTransceiver<PayloadFlightPhases, SimulatorData,
+                                    ActuatorData>::HILTransceiver;
+};
+
+class PayloadHILPhasesManager
+    : public Boardcore::HILPhasesManager<PayloadFlightPhases, SimulatorData,
+                                         ActuatorData>
+{
+public:
+    explicit PayloadHILPhasesManager(
+        std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition);
+
+    void processFlagsImpl(
+        const SimulatorData& simulatorData,
+        std::vector<PayloadFlightPhases>& changed_flags) override;
+
+    void printOutcomes();
+
+private:
+    void handleEventImpl(
+        const Boardcore::Event& e,
+        std::vector<PayloadFlightPhases>& changed_flags) override;
+};
+
+class PayloadHIL
+    : public Boardcore::HIL<PayloadFlightPhases, SimulatorData, ActuatorData>,
+      public Boardcore::InjectableWithDeps<
+          Payload::Buses, Payload::Actuators, Payload::FlightModeManager,
+          Payload::WindEstimation, Payload::WingController,
+          Payload::NASController>
+{
+
+public:
+    PayloadHIL();
+
+    bool start() override;
+
+private:
+    ActuatorData updateActuatorData();
+};
+
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/HIL/HILData.h b/src/boards/Payload/HIL/HILData.h
new file mode 100644
index 000000000..71f0f04d9
--- /dev/null
+++ b/src/boards/Payload/HIL/HILData.h
@@ -0,0 +1,259 @@
+/* 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 <Payload/Configs/HILSimulationConfig.h>
+#include <sensors/HILSimulatorData.h>
+
+// NAS
+#include <Payload/StateMachines/NASController/NASControllerData.h>
+#include <algorithms/NAS/NASState.h>
+
+// WingController
+#include <Payload/StateMachines/WingController/WingControllerData.h>
+
+namespace Payload
+{
+
+// Sensors Data
+using PayloadAccelerometerSimulatorData =
+    Boardcore::AccelerometerSimulatorData<Config::HIL::N_DATA_ACCEL>;
+using PayloadGyroscopeSimulatorData =
+    Boardcore::GyroscopeSimulatorData<Config::HIL::N_DATA_GYRO>;
+using PayloadMagnetometerSimulatorData =
+    Boardcore::MagnetometerSimulatorData<Config::HIL::N_DATA_MAGNETO>;
+using PayloadGPSSimulatorData =
+    Boardcore::GPSSimulatorData<Config::HIL::N_DATA_GPS>;
+using PayloadDigitalBarometerSimulatorData =
+    Boardcore::BarometerSimulatorData<Config::HIL::N_DATA_BARO_STATIC>;
+using PayloadAnalogBarometerSimulatorData =
+    Boardcore::BarometerSimulatorData<Config::HIL::N_DATA_BARO_PITOT>;
+using PayloadTemperatureSimulatorData =
+    Boardcore::TemperatureSimulatorData<Config::HIL::N_DATA_TEMP>;
+
+enum class HILSignal : int
+{
+    SIMULATION_STARTED      = 1,
+    SIMULATION_STOPPED      = 2,
+    SIMULATION_FORCE_LAUNCH = 3
+};
+
+enum class PayloadFlightPhases
+{
+    SIMULATION_STARTED,
+    INITIALIZED,
+    CALIBRATION,
+    CALIBRATION_OK,
+    ARMED,
+    LIFTOFF_PIN_DETACHED,
+    LIFTOFF,
+    MOTOR_SHUTDOWN,
+    AEROBRAKES,
+    APOGEE,
+    PARA1,
+    PARA2,
+    SIMULATION_STOPPED
+};
+
+/**
+ * @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(0),
+          updating(0)
+    {
+    }
+
+    explicit NASStateHIL(const Boardcore::NASState& nasState)
+        : n(nasState.n), e(nasState.e), d(nasState.d), vn(nasState.vn),
+          ve(nasState.ve), vd(nasState.vd), qx(nasState.qx), qy(nasState.qy),
+          qz(nasState.qz), qw(nasState.qw), updating(1.f)  // To remove updating
+    {
+    }
+
+    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);
+    }
+};
+
+struct ActuatorsStateHIL
+{
+    float parafoilLeftPercentage  = 0;
+    float parafoilRightPercentage = 0;
+    float cutterState             = 0;
+
+    ActuatorsStateHIL()
+        : parafoilLeftPercentage(0.0f), parafoilRightPercentage(0.0f),
+          cutterState(0.0f)
+    {
+    }
+
+    ActuatorsStateHIL(float parafoilLeftPercentage,
+                      float parafoilRightPercentage, float cutterState)
+        : parafoilLeftPercentage(parafoilLeftPercentage),
+          parafoilRightPercentage(parafoilRightPercentage),
+          cutterState(cutterState)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "parafoilLeft: %f perc\n"
+            "parafoilRight: %f perc\n"
+            "cutterState: %f perc\n",
+            parafoilLeftPercentage * 100, parafoilRightPercentage * 100,
+            cutterState * 100);
+    }
+};
+
+struct WESDataHIL
+{
+    float windX;
+    float windY;
+
+    explicit WESDataHIL(const Eigen::Vector2f& wind)
+        : windX(wind[0]), windY(wind[1])
+    {
+    }
+
+    WESDataHIL() : windX(0.0f), windY(0.0f) {}
+
+    void print() { printf("wind: [%f,%f]\n", windX, windY); }
+};
+
+struct GuidanceDataHIL
+{
+    float psiRef;
+    float deltaA;
+    float currentTargetN;
+    float currentTargetE;
+
+    GuidanceDataHIL(float psiRef, float deltaA, float currentTargetN,
+                    float currentTargetE)
+        : psiRef(psiRef), deltaA(deltaA), currentTargetN(currentTargetN),
+          currentTargetE(currentTargetE)
+    {
+    }
+
+    GuidanceDataHIL()
+        : psiRef(0.0f), deltaA(0.0f), currentTargetN(0), currentTargetE(0)
+    {
+    }
+
+    void print()
+    {
+        printf(
+            "psiRef: %f\n"
+            "deltaA: %f\n"
+            "currentTargetN: %f\n"
+            "currentTargetE: %f\n",
+            psiRef, deltaA, currentTargetN, currentTargetE);
+    }
+};
+
+/**
+ * @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
+{
+    PayloadAccelerometerSimulatorData accelerometer;
+    PayloadGyroscopeSimulatorData gyro;
+    PayloadMagnetometerSimulatorData magnetometer;
+    PayloadGPSSimulatorData gps;
+    PayloadDigitalBarometerSimulatorData barometer1, barometer2, barometer3;
+    PayloadAnalogBarometerSimulatorData staticPitot, dynamicPitot;
+    PayloadTemperatureSimulatorData temperature;
+    float signal;
+};
+
+/**
+ * @brief Data strudcture expected by the simulator
+ */
+struct ActuatorData
+{
+    NASStateHIL nasState;
+    ActuatorsStateHIL actuatorsState;
+    WESDataHIL wesData;
+    GuidanceDataHIL guidanceData;
+    float signal;
+
+    ActuatorData()
+        : nasState(), actuatorsState(), wesData(), guidanceData(), signal(0)
+    {
+    }
+
+    ActuatorData(const NASStateHIL& nasState,
+                 const ActuatorsStateHIL& actuatorsState,
+                 const WESDataHIL& wesData, const GuidanceDataHIL& guidanceData,
+                 float signal)
+        : nasState(nasState), actuatorsState(actuatorsState), wesData(wesData),
+          guidanceData(guidanceData), signal(signal)
+    {
+    }
+
+    void print()
+    {
+        nasState.print();
+        actuatorsState.print();
+        wesData.print();
+        guidanceData.print();
+    }
+};
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/PersistentVars/PersistentVars.cpp b/src/boards/Payload/PersistentVars/PersistentVars.cpp
new file mode 100644
index 000000000..4d59bae1a
--- /dev/null
+++ b/src/boards/Payload/PersistentVars/PersistentVars.cpp
@@ -0,0 +1,48 @@
+/* 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 "PersistentVars.h"
+
+#include <arch/common/drivers/stm32_bsram.h>
+#include <miosix.h>
+
+using namespace miosix;
+
+static bool PRESERVE hilMode = false;
+
+namespace Payload
+{
+
+namespace PersistentVars
+{
+
+void setHilMode(bool _hilMode)
+{
+    BSRAM::EnableWriteLock l;
+    hilMode = _hilMode;
+}
+
+bool getHilMode() { return hilMode; }
+
+}  // namespace PersistentVars
+
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/PersistentVars/PersistentVars.h b/src/boards/Payload/PersistentVars/PersistentVars.h
new file mode 100644
index 000000000..9f2ed5db5
--- /dev/null
+++ b/src/boards/Payload/PersistentVars/PersistentVars.h
@@ -0,0 +1,37 @@
+/* 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
+
+namespace Payload
+{
+
+namespace PersistentVars
+{
+
+void setHilMode(bool _hilMode);
+
+bool getHilMode();
+
+}  // namespace PersistentVars
+
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index cefef485c..11bae58af 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -613,7 +613,7 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
             tm.motor_can_status = canStatus.isMotorConnected();
             tm.rig_can_status   = canStatus.isRigConnected();
 
-            tm.hil_state = 0;  // TODO: hil
+            tm.hil_state = PersistentVars::getHilMode();
 
             mavlink_msg_payload_stats_tm_encode(config::Mavlink::SYSTEM_ID,
                                                 config::Mavlink::COMPONENT_ID,
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 8c1566d09..f82d18870 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -98,7 +98,9 @@ bool Radio::start()
         return false;
     }
 
-    if (config::MAVLINK_OVER_HIL_SERIAL_ENABLED)
+    // Enable Mavlink over HIL USART only when HIL is not active
+    if (config::MAVLINK_OVER_HIL_SERIAL_ENABLED &&
+        !PersistentVars::getHilMode())
     {
         initMavlinkOverSerial();
     }
@@ -132,8 +134,6 @@ bool Radio::isStarted() { return started; }
 
 void Radio::initMavlinkOverSerial()
 {
-    // Send Mavlink messages over the HIL USART when HIL is not active
-    // TODO: hil - don't use serial if hil is active
     serialTransceiver =
         std::make_unique<SerialTransceiver>(getModule<Buses>()->HILUart());
 
diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h
index 637c46629..6353e4d72 100644
--- a/src/boards/Payload/Radio/Radio.h
+++ b/src/boards/Payload/Radio/Radio.h
@@ -23,6 +23,7 @@
 #pragma once
 
 #include <Payload/Configs/RadioConfig.h>
+#include <Payload/PersistentVars/PersistentVars.h>
 #include <common/Mavlink.h>
 #include <radio/MavlinkDriver/MavlinkDriver.h>
 #include <radio/SX1278/SX1278Fsk.h>
diff --git a/src/boards/Payload/Sensors/HILSensors.h b/src/boards/Payload/Sensors/HILSensors.h
new file mode 100644
index 000000000..0ae5ab621
--- /dev/null
+++ b/src/boards/Payload/Sensors/HILSensors.h
@@ -0,0 +1,253 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: 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 <Payload/HIL/HIL.h>
+#include <common/CanConfig.h>
+#include <common/ReferenceConfig.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <sensors/HILSensor.h>
+#include <sensors/Sensor.h>
+
+#include "Sensors.h"
+
+namespace Payload
+{
+
+class HILSensors
+    : public Boardcore::InjectableWithDeps<Boardcore::InjectableBase<Sensors>,
+                                           PayloadHIL>
+{
+public:
+    explicit HILSensors(bool enableHw) : Super{}, enableHw{enableHw} {}
+
+private:
+    bool postSensorCreationHook() override
+    {
+        using namespace Boardcore;
+
+        hillificator<>(lps22df, enableHw,
+                       [this]() { return updateLPS22DFData(); });
+        hillificator<>(lps28dfw, enableHw,
+                       [this]() { return updateLPS28DFWData(); });
+        hillificator<>(h3lis331dl, enableHw,
+                       [this]() { return updateH3LIS331DLData(); });
+        hillificator<>(lis2mdl, enableHw,
+                       [this]() { return updateLIS2MDLData(); });
+        hillificator<>(ubxgps, enableHw,
+                       [this]() { return updateUBXGPSData(); });
+        hillificator<>(lsm6dsrx, enableHw,
+                       [this]() { return updateLSM6DSRXData(); });
+        hillificator<>(staticPressure, enableHw,
+                       [this]() { return updateStaticPressureData(); });
+        hillificator<>(dynamicPressure, enableHw,
+                       [this]() { return updateDynamicPressureData(); });
+        hillificator<>(rotatedImu, enableHw,
+                       [this]() { return updateIMUData(); });
+
+        return true;
+    }
+
+    int getSampleCounter(int nData)
+    {
+        auto ts = miosix::getTime();
+        auto tsSensorData =
+            getModule<PayloadHIL>()->getTimestampSimulatorData();
+        auto simulationPeriod = getModule<PayloadHIL>()->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;
+    }
+
+    Boardcore::LPS28DFWData updateLPS28DFWData()
+    {
+        Boardcore::LPS28DFWData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
+        int iTemp = getSampleCounter(sensorData->temperature.NDATA);
+
+        data.pressureTimestamp = data.temperatureTimestamp =
+            Boardcore::TimestampTimer::getTimestamp();
+        data.pressure    = sensorData->barometer1.measures[iBaro];
+        data.temperature = sensorData->temperature.measures[iTemp];
+
+        return data;
+    };
+
+    Boardcore::LPS22DFData updateLPS22DFData()
+    {
+        Boardcore::LPS22DFData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->barometer1.NDATA);
+        int iTemp = getSampleCounter(sensorData->temperature.NDATA);
+
+        data.pressureTimestamp = data.temperatureTimestamp =
+            Boardcore::TimestampTimer::getTimestamp();
+        data.pressure    = sensorData->barometer1.measures[iBaro];
+        data.temperature = sensorData->temperature.measures[iTemp];
+
+        return data;
+    };
+
+    Boardcore::H3LIS331DLData updateH3LIS331DLData()
+    {
+        Boardcore::H3LIS331DLData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iAcc = getSampleCounter(sensorData->accelerometer.NDATA);
+
+        data.accelerationTimestamp = Boardcore::TimestampTimer::getTimestamp();
+        data.accelerationX = sensorData->accelerometer.measures[iAcc][0];
+        data.accelerationY = sensorData->accelerometer.measures[iAcc][1];
+        data.accelerationZ = sensorData->accelerometer.measures[iAcc][2];
+
+        return data;
+    };
+
+    Boardcore::LIS2MDLData updateLIS2MDLData()
+    {
+        Boardcore::LIS2MDLData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iMag = getSampleCounter(sensorData->magnetometer.NDATA);
+
+        data.magneticFieldTimestamp = Boardcore::TimestampTimer::getTimestamp();
+        data.magneticFieldX = sensorData->magnetometer.measures[iMag][0];
+        data.magneticFieldY = sensorData->magnetometer.measures[iMag][1];
+        data.magneticFieldZ = sensorData->magnetometer.measures[iMag][2];
+
+        return data;
+    };
+
+    Boardcore::UBXGPSData updateUBXGPSData()
+    {
+        Boardcore::UBXGPSData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iGps = getSampleCounter(sensorData->gps.NDATA);
+
+        data.gpsTimestamp = Boardcore::TimestampTimer::getTimestamp();
+
+        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;
+    };
+
+    Boardcore::LSM6DSRXData updateLSM6DSRXData()
+    {
+        Boardcore::LSM6DSRXData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iAcc  = getSampleCounter(sensorData->accelerometer.NDATA);
+        int iGyro = getSampleCounter(sensorData->gyro.NDATA);
+
+        data.accelerationTimestamp = data.angularSpeedTimestamp =
+            Boardcore::TimestampTimer::getTimestamp();
+
+        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;
+    };
+
+    Boardcore::MPXH6115AData updateStaticPressureData()
+    {
+        Boardcore::MPXH6115AData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->staticPitot.NDATA);
+
+        data.pressureTimestamp = Boardcore::TimestampTimer::getTimestamp();
+        data.pressure          = sensorData->staticPitot.measures[iBaro];
+
+        return data;
+    };
+
+    Boardcore::MPXH6115AData updateDynamicPressureData()
+    {
+        Boardcore::MPXH6115AData data;
+
+        auto* sensorData = getModule<PayloadHIL>()->getSensorData();
+
+        int iBaro = getSampleCounter(sensorData->dynamicPitot.NDATA);
+
+        data.pressureTimestamp = Boardcore::TimestampTimer::getTimestamp();
+        data.pressure          = sensorData->dynamicPitot.measures[iBaro];
+
+        return data;
+    };
+
+    Boardcore::IMUData updateIMUData()
+    {
+        return Boardcore::IMUData{getLSM6DSRXLastSample(),
+                                  getLSM6DSRXLastSample(),
+                                  getCalibratedLIS2MDLLastSample()};
+    };
+
+    bool enableHw;
+};
+}  // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
index 88af73d21..657dbf8c6 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -42,6 +42,22 @@ namespace config = Payload::Config::FlightModeManager;
 namespace Payload
 {
 
+void enterHilMode()
+{
+    PersistentVars::setHilMode(true);
+    miosix::reboot();
+}
+
+void exitHilMode()
+{
+    // Reboot only if in HIL mode
+    if (PersistentVars::getHilMode())
+    {
+        PersistentVars::setHilMode(false);
+        miosix::reboot();
+    }
+}
+
 FlightModeManager::FlightModeManager()
     : HSM(&FlightModeManager::OnGround, miosix::STACK_DEFAULT_FOR_PTHREAD,
           BoardScheduler::flightModeManagerPriority())
@@ -101,6 +117,20 @@ State FlightModeManager::OnGround(const Event& event)
             return HANDLED;
         }
 
+        case TMTC_EXIT_HIL_MODE:
+        {
+            getModule<CanHandler>()->sendEvent(
+                CanConfig::EventId::EXIT_HIL_MODE);
+            miosix::Thread::sleep(1000);
+            exitHilMode();
+            return HANDLED;
+        }
+        case CAN_EXIT_HIL_MODE:
+        {
+            exitHilMode();
+            return HANDLED;
+        }
+
         case TMTC_RESET_BOARD:
         {
             Logger::getInstance().stop();
@@ -428,6 +458,20 @@ State FlightModeManager::OnGroundTestMode(const Event& event)
             return HANDLED;
         }
 
+        case TMTC_ENTER_HIL_MODE:
+        {
+            getModule<CanHandler>()->sendEvent(
+                CanConfig::EventId::ENTER_HIL_MODE);
+            miosix::Thread::sleep(1000);
+            enterHilMode();
+            return HANDLED;
+        }
+        case CAN_ENTER_HIL_MODE:
+        {
+            enterHilMode();
+            return HANDLED;
+        }
+
         case TMTC_EXIT_TEST_MODE:
         {
             getModule<CanHandler>()->sendEvent(
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
index 3367c7e85..ecbf30878 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <Payload/PersistentVars/PersistentVars.h>
 #include <diagnostic/PrintLogger.h>
 #include <events/HSM.h>
 #include <utils/DependencyManager/DependencyManager.h>
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index 86fd32df8..90c4c6ff6 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -28,8 +28,11 @@
 #include <Payload/Buses.h>
 #include <Payload/CanHandler/CanHandler.h>
 #include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Payload/HIL/HIL.h>
+#include <Payload/PersistentVars/PersistentVars.h>
 #include <Payload/PinHandler/PinHandler.h>
 #include <Payload/Radio/Radio.h>
+#include <Payload/Sensors/HILSensors.h>
 #include <Payload/Sensors/Sensors.h>
 #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <Payload/StateMachines/NASController/NASController.h>
@@ -130,7 +133,9 @@ int main()
     initResult &= depman.insert(nasController);
 
     // Sensors
-    auto sensors = new Sensors();
+    auto sensors =
+        (PersistentVars::getHilMode() ? new HILSensors(Config::HIL::ENABLE_HW)
+                                      : new Sensors());
     initResult &= depman.insert(sensors);
     auto pinHandler = new PinHandler();
     initResult &= depman.insert(pinHandler);
@@ -157,6 +162,17 @@ int main()
     auto statsRecorder = new FlightStatsRecorder();
     initResult &= depman.insert(statsRecorder);
 
+    // HIL
+    PayloadHIL* hil = nullptr;
+    if (PersistentVars::getHilMode())
+    {
+        std::cout << "PAYLOAD SimulatorData: " << sizeof(SimulatorData)
+                  << ", ActuatorData: " << sizeof(ActuatorData) << std::endl;
+
+        hil = new PayloadHIL();
+        initResult &= depman.insert(hil);
+    }
+
     std::cout << "Injecting module dependencies" << std::endl;
     initResult &= depman.inject();
 
@@ -176,7 +192,6 @@ int main()
     START_SINGLETON(EventBroker);
 
     // Start module instances
-    START_MODULE(sensors) { miosix::led1On(); }
     START_MODULE(pinHandler);
     START_MODULE(radio) { miosix::led2On(); }
     START_MODULE(canHandler) { miosix::led3On(); }
@@ -199,6 +214,18 @@ int main()
             Logger::getInstance().log(ev);
         });
 
+    if (hil)
+    {
+        START_MODULE(hil);
+
+        std::cout << "Waiting start simulation" << std::endl;
+        hil->waitStartSimulation();
+    }
+
+    // Wait for simulation start before starting sensors to avoid initializing
+    // them with invalid data
+    START_MODULE(sensors) { miosix::led1On(); }
+
     if (initResult)
     {
         EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
-- 
GitLab