diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 4b020343a7afcbcdf7f2ad77dadb3ca22c89088c..78fa0a5bf72b56797516944e56d65f09ad4c7863 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -74,6 +74,7 @@
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "_MIOSIX",
+                "HILSimulation",
                 "__cplusplus=201103L"
             ],
             "includePath": [
@@ -185,6 +186,70 @@
             },
             "configurationProvider": "ms-vscode.cmake-tools"
         },
+        {
+            "name": "stm32f429zi_skyward_death_stack_x_maker_faire",
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "DEBUG",
+                "HILSimulation",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X_MAKER_FAIRE",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+            "includePath": [
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/skyward-boardcore/libs/Catch2/single_include",
+                "${workspaceFolder}/skyward-boardcore/libs/mavlink-skyward-lib",
+                "${workspaceFolder}/skyward-boardcore/libs/fmt/include",
+                "${workspaceFolder}/skyward-boardcore/libs/eigen",
+                "${workspaceFolder}/skyward-boardcore/libs/tscpp",
+                "${workspaceFolder}/skyward-boardcore/libs",
+                "${workspaceFolder}/skyward-boardcore/src/shared",
+                "${workspaceFolder}/skyward-boardcore/src/tests",
+                "${workspaceFolder}/src/hardware_in_the_loop",
+                "${workspaceFolder}/src/boards",
+                "${workspaceFolder}/src/tests",
+                "${workspaceFolder}/src"
+            ],
+            "browse": {
+                "path": [
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix",
+                    "${workspaceFolder}/skyward-boardcore/libs/mavlink-skyward-lib",
+                    "${workspaceFolder}/skyward-boardcore/libs/eigen",
+                    "${workspaceFolder}/skyward-boardcore/libs/tscpp",
+                    "${workspaceFolder}/skyward-boardcore/libs/mxgui",
+                    "${workspaceFolder}/skyward-boardcore/libs/fmt",
+                    "${workspaceFolder}/skyward-boardcore/src/shared",
+                    "${workspaceFolder}/skyward-boardcore/src/tests",
+                    "${workspaceFolder}/src/hardware_in_the_loop",
+                    "${workspaceFolder}/src/boards",
+                    "${workspaceFolder}/src/tests"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "configurationProvider": "ms-vscode.cmake-tools"
+        },
         {
             "name": "stm32f429zi_skyward_death_stack_v3",
             "cStandard": "c11",
@@ -683,4 +748,4 @@
         }
     ],
     "version": 4
-}
\ No newline at end of file
+}
diff --git a/.vscode/settings.json b/.vscode/settings.json
index d64d9495c135a6f23bd27faa31950c80082b3441..743392d0cbad610129652fe8bcf299b7831f3e7f 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,10 +1,7 @@
 {
-    "cmake.configureSettings": {
-        "CMAKE_C_COMPILER_LAUNCHER": "ccache",
-        "CMAKE_CXX_COMPILER_LAUNCHER": "ccache"
-    },
     "cmake.parallelJobs": 1,
     "files.associations": {
+        "**/miosix-kernel/**": "c",
         "sstream": "cpp",
         "format": "cpp",
         "any": "cpp",
@@ -92,7 +89,38 @@
         "specialfunctions": "cpp",
         "splines": "cpp",
         "matrixfunctions": "cpp",
-        "tensorsymmetry": "cpp"
+        "tensorsymmetry": "cpp",
+        "ios": "cpp",
+        "charconv": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "forward_list": "cpp",
+        "locale": "cpp",
+        "queue": "cpp",
+        "stop_token": "cpp",
+        "xfacet": "cpp",
+        "xhash": "cpp",
+        "xiosbase": "cpp",
+        "xlocale": "cpp",
+        "xlocbuf": "cpp",
+        "xlocinfo": "cpp",
+        "xlocmes": "cpp",
+        "xlocmon": "cpp",
+        "xlocnum": "cpp",
+        "xloctime": "cpp",
+        "xmemory": "cpp",
+        "xstddef": "cpp",
+        "xstring": "cpp",
+        "xtr1common": "cpp",
+        "xtree": "cpp",
+        "xutility": "cpp"
+    },
+    "editor.formatOnSave": true,
+    "[c]": {
+        "editor.formatOnSave": false
+    },
+    "[cmake]": {
+        "editor.formatOnSave": false
     },
     "cSpell.words": [
         "Aeroutils",
@@ -138,5 +166,9 @@
         "VREF",
         "Xbee"
     ],
-    "C_Cpp.errorSquiggles": "Enabled"
-}
+    "C_Cpp.errorSquiggles": "Enabled",
+    "cmake.configureSettings": {
+        "CMAKE_C_COMPILER_LAUNCHER": "ccache",
+        "CMAKE_CXX_COMPILER_LAUNCHER": "ccache"
+    }
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e85e8d7a62ed79213cfd0bd77d90b63fb3457103..1bee016e7317b5bd31b5b4051942f14b80ec3036 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -54,19 +54,26 @@ sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW)
+target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP)
 sbs_target(main-entry-hil-euroc stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW)
+target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP)
 sbs_target(main-entry-hil-roccaraso stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-milano src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-milano PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW)
+target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW USE_SERIAL_TRANSCEIVER)
 sbs_target(main-entry-hil-milano stm32f429zi_skyward_death_stack_v3)
 
+add_executable(main-entry-hil-maker-faire src/entrypoints/Main/main-entry-maker-faire.cpp ${MAIN_COMPUTER} ${HIL})
+# add_executable(main-entry-hil-maker-faire src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
+target_include_directories(main-entry-hil-maker-faire PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(main-entry-hil-maker-faire PRIVATE HILSimulation EUROC BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP DEATHSTACK_V2)
+sbs_target(main-entry-hil-maker-faire stm32f429zi_skyward_death_stack_x_maker_faire)
+# sbs_target(main-entry-hil-maker-faire stm32f429zi_stm32f4discovery)
+
 add_executable(payload-entry-milano src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER})
 target_include_directories(payload-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS})
 target_compile_definitions(payload-entry-milano PRIVATE MILANO)
@@ -86,6 +93,11 @@ add_executable(ciuti-entry src/entrypoints/Ciuti/ciuti-entry.cpp ${CIUTI_COMPUTE
 target_include_directories(ciuti-entry PRIVATE ${OBSW_INCLUDE_DIRS})
 sbs_target(ciuti-entry stm32f205rc_skyward_ciuti)
 
+add_executable(payload-entry-hil src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER} ${HIL})
+target_include_directories(payload-entry-hil PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(payload-entry-hil PRIVATE PAYLOAD_ENTRY HILSimulation EUROC USE_SERIAL_TRANSCEIVER)
+sbs_target(payload-entry-hil stm32f429zi_skyward_death_stack_x)
+
 add_executable(groundstation-entry src/entrypoints/Groundstation/groundstation-entry.cpp)
 sbs_target(groundstation-entry stm32f429zi_skyward_groundstation_v2)
 
@@ -118,7 +130,12 @@ add_executable(main-test-cutter src/tests/Main/actuators/test-cutter.cpp)
 sbs_target(main-test-cutter stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-test-servos src/tests/Main/actuators/test-servos.cpp)
-sbs_target(main-test-servos stm32f429zi_skyward_death_stack_v3)
+sbs_target(main-test-servos stm32f429zi_skyward_death_stack_x_maker_faire)
+
+add_executable(test-airbrakes-st src/tests/Main/actuators/test-airbrakes-st.cpp ${MAIN_COMPUTER} ${HIL})
+target_include_directories(test-airbrakes-st PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(test-airbrakes-st PRIVATE HILSimulation)
+sbs_target(test-airbrakes-st stm32f429zi_skyward_death_stack_x_maker_faire)
 
 add_executable(main-test-bmx160-calibration src/tests/Main/calibration/test-bmx160-calibration.cpp)
 sbs_target(main-test-bmx160-calibration stm32f429zi_skyward_death_stack_v3)
diff --git a/skyward-boardcore b/skyward-boardcore
index 0030a2bb5e8e62d56d28bd5bdcb34a9e58ebd499..7638dc18607fc870a7550a0691b0edb47592a2c0 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 0030a2bb5e8e62d56d28bd5bdcb34a9e58ebd499
+Subproject commit 7638dc18607fc870a7550a0691b0edb47592a2c0
diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp
index 5d76d1adad3320f292e99fa4940133575264fa07..7871c6a4a6ebc731d43c25168b62f960058dd4e6 100644
--- a/src/boards/Main/Actuators/Actuators.cpp
+++ b/src/boards/Main/Actuators/Actuators.cpp
@@ -121,10 +121,10 @@ bool Actuators::disableServo(ServosList servoId)
     switch (servoId)
     {
         case AIR_BRAKES_SERVO:
-            servoAirbrakes.enable();
+            servoAirbrakes.disable();
             break;
         case EXPULSION_SERVO:
-            servoAirbrakes.enable();
+            servoAirbrakes.disable();
             break;
         default:
             return false;
diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h
index d586ffac3517f015a9be6da4f7d0e1f20566c065..4d845b69180f07a347813d71b061d82cfa4d50e1 100644
--- a/src/boards/Main/Buses.h
+++ b/src/boards/Main/Buses.h
@@ -67,8 +67,9 @@ private:
           uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200), spi1({}),
           spi2({}), spi4({}), spi5({}), spi6({})
     {
-        // usart2.init();
+        usart2.init();
         usart3.init();
+        uart4.init();
     }
 #endif
 };
diff --git a/src/boards/Main/Configs/ActuatorsConfigs.h b/src/boards/Main/Configs/ActuatorsConfigs.h
index 2a8bd2e7b6ca9ba3dabb870de48c6864ed55a464..dc6008213fabab0c58e7716de02d8163a4130381 100644
--- a/src/boards/Main/Configs/ActuatorsConfigs.h
+++ b/src/boards/Main/Configs/ActuatorsConfigs.h
@@ -32,6 +32,20 @@ namespace ActuatorsConfigs
 {
 
 // Airbrakes servo
+#ifdef DEATHSTACK_V2
+// calibrated at
+//  constexpr float ABK_SERVO_MIN_PULSE = 1080;  // [us]
+//  constexpr float ABK_SERVO_MAX_PULSE = 1600;  // [us]
+
+static TIM_TypeDef* const ABK_SERVO_TIMER = TIM8;
+constexpr Boardcore::TimerUtils::Channel ABK_SERVO_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+
+constexpr float ABK_SERVO_ROTATION  = 53;    // [deg] AirBrakes without end stop
+constexpr float ABK_SERVO_MIN_PULSE = 1080;  // [us]
+constexpr float ABK_SERVO_MAX_PULSE = 1500;  // [us]
+#else
+// TODO: Fix rotation value
 static TIM_TypeDef* const ABK_SERVO_TIMER = TIM10;
 constexpr Boardcore::TimerUtils::Channel ABK_SERVO_PWM_CH =
     Boardcore::TimerUtils::Channel::CHANNEL_1;
@@ -40,7 +54,9 @@ constexpr float ABK_SERVO_ROTATION  = 66.4;  // [deg] AirBrakes without end stop
 constexpr float ABK_SERVO_MIN_PULSE = 1405;  // [deg]
 constexpr float ABK_SERVO_MAX_PULSE =
     ABK_SERVO_MIN_PULSE - 10 * ABK_SERVO_ROTATION;  // [us]
-constexpr float ABK_WIGGLE_TIME = 2 * 1000;         // [ms]
+#endif
+
+constexpr float ABK_WIGGLE_TIME = 2 * 1000;  // [ms]
 
 // Deployment servo
 static TIM_TypeDef* const DPL_SERVO_TIMER = TIM4;
@@ -55,9 +71,16 @@ constexpr float DPL_SERVO_RESET_POS = DPL_SERVO_ROTATION;             // [deg]
 constexpr float DPL_WIGGLE_TIME     = 5 * 1000;                       // [ms]
 
 // Buzzer
+
+#ifdef DEATHSTACK_V2
+TIM_TypeDef* const BUZZER_TIMER = TIM1;
+constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+#else
 TIM_TypeDef* const BUZZER_TIMER = TIM8;
 constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
     Boardcore::TimerUtils::Channel::CHANNEL_1;
+#endif
 
 #ifdef BUZZER_LOW
 constexpr float BUZZER_DUTY_CYCLE = 0.01;
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfig.h b/src/boards/Main/Configs/AirBrakesControllerConfig.h
index 4480be7084de1e32db6ead281cb3e467a58e1d6b..29b0d580d7cc467afac80376ea171331ae8bbffe 100644
--- a/src/boards/Main/Configs/AirBrakesControllerConfig.h
+++ b/src/boards/Main/Configs/AirBrakesControllerConfig.h
@@ -74,9 +74,6 @@ static const Boardcore::AirBrakesConfig ABK_CONFIG{
     .S0         = 0.017671458676443,
     .SURFACE    = 0.009564 * Main::ActuatorsConfigs::ABK_SERVO_ROTATION *
                EIGEN_PI / 180.0,
-    .KP = 20,
-    .KI = 5,
-    .TS = UPDATE_PERIOD / 1000.0,
 };
 
 }  // namespace AirBrakesControllerConfig
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae0d3aa5234c950f3ac56ff583f161928311ca55
--- /dev/null
+++ b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
@@ -0,0 +1,41 @@
+/* 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 <Main/Configs/AirBrakesControllerConfig.h>
+#include <algorithms/AirBrakes/AirBrakesInterpConfig.h>
+
+namespace Main
+{
+
+namespace AirBrakesControllerConfig
+{
+
+static const Boardcore::AirBrakesInterpConfig ABK_CONFIG_INTERP{
+    .INITIAL_FILTER_COEFF = 0.3,
+    .INITIAL_T_FILTER     = 12,
+    .DELTA_T_FILTER       = 2.5,
+    .FILTER_RATIO         = 2};
+}  // namespace AirBrakesControllerConfig
+
+}  // namespace Main
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigPI.h b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2696756f531b63b310d053a598bee81640d6540
--- /dev/null
+++ b/src/boards/Main/Configs/AirBrakesControllerConfigPI.h
@@ -0,0 +1,39 @@
+/* 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 <Main/Configs/AirBrakesControllerConfig.h>
+#include <algorithms/AirBrakes/AirBrakesPIConfig.h>
+
+namespace Main
+{
+
+namespace AirBrakesControllerConfig
+{
+
+static const Boardcore::AirBrakesPIConfig ABK_CONFIG_PI{
+    .KP = 20, .KI = 5, .TS = UPDATE_PERIOD / 1000.0};
+
+}  // namespace AirBrakesControllerConfig
+
+}  // namespace Main
diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
index 43c7d5d802e022a8fb9e4e858f1999d8339d0838..3b5cf64b9518fe37d833d9d003feb607345036b1 100644
--- a/src/boards/Main/Configs/SensorsConfig.h
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -24,7 +24,6 @@
 
 #include <drivers/adc/InternalADC.h>
 #include <drivers/usart/USART.h>
-#include <interfaces-impl/hwmapping.h>
 #include <sensors/ADS131M04/ADS131M04.h>
 #include <sensors/BMX160/BMX160Config.h>
 #include <sensors/LIS3MDL/LIS3MDL.h>
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index fff8510746d0d0d28ccdda6c13df8b9f8ceb3a38..d04033c35fcb56e3bcddd6082b312629074101fe 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -34,6 +34,7 @@
 #include <Main/TMRepository/TMRepository.h>
 #include <common/events/Events.h>
 #include <drivers/interrupt/external_interrupts.h>
+#include <interfaces-impl/hwmapping.h>
 
 #include <functional>
 
@@ -94,7 +95,7 @@ Radio::Radio()
 {
 #if defined(USE_SERIAL_TRANSCEIVER)
     Boardcore::SerialTransceiver* transceiver;
-    transceiver = new SerialTransceiver(Buses::getInstance().usart1);
+    transceiver = new SerialTransceiver(Buses::getInstance().usart2);
 #elif defined(USE_XBEE_TRANSCEIVER)
     SPIBusConfig config;
     config.clockDivider = SPI::ClockDivider::DIV_16;
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index 0bcbb26135a5549f8d3995854165ce27b6ad2532..21a98a1cd79b7f5b44e717f142913daf3f50a255 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -28,6 +28,7 @@
 #include <common/events/Events.h>
 #include <drivers/interrupt/external_interrupts.h>
 #include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
 
 using namespace std;
 using namespace miosix;
diff --git a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
index ff64e2e484392a5855be473053c3eca797a9e13e..8996ae207a1e9b2a3f0a8f1aeb94d6cf6e78bfea 100644
--- a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
+++ b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
@@ -25,8 +25,9 @@
 #include <Main/Actuators/Actuators.h>
 #include <Main/BoardScheduler.h>
 #include <Main/Configs/ActuatorsConfigs.h>
-#include <Main/Configs/AirBrakesControllerConfig.h>
 #include <Main/StateMachines/NASController/NASController.h>
+#include <algorithms/AirBrakes/AirBrakesInterp.h>
+#include <algorithms/AirBrakes/AirBrakesPI.h>
 #include <common/events/Events.h>
 #include <drivers/timer/TimestampTimer.h>
 #include <events/EventBroker.h>
@@ -36,8 +37,10 @@
 #endif
 
 #ifdef INTERP
+#include "Main/Configs/AirBrakesControllerConfigInterp.h"
 #include "TrajectorySetInterp.h"
 #else
+#include "Main/Configs/AirBrakesControllerConfigPI.h"
 #include "TrajectorySet.h"
 #endif
 
@@ -125,6 +128,10 @@ void AirBrakesController::state_idle(const Event& event)
         }
         case FLIGHT_LIFTOFF:
         {
+#ifdef INTERP
+            abk.setLiftoffTimestamp();
+#endif
+
             return transition(&AirBrakesController::state_shadow_mode);
         }
     }
@@ -190,6 +197,7 @@ void AirBrakesController::state_end(const Event& event)
 AirBrakesController::AirBrakesController()
     : FSM(&AirBrakesController::state_init),
       abk(
+#ifndef INTERP
 #ifndef HILMockNAS
           []() {
               return TimedTrajectoryPoint{
@@ -199,10 +207,28 @@ AirBrakesController::AirBrakesController()
           []() { return Sensors::getInstance().state.kalman->getLastSample(); },
 #endif  // HILMockNAS
           TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG,
+          AirBrakesControllerConfig::ABK_CONFIG_PI,
           [](float position) {
               Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO,
                                                 position);
           })
+#else  // INTERP
+#ifndef HILMockNAS
+          []() {
+              return TimedTrajectoryPoint{
+                  NASController::getInstance().getNasState()};
+          },
+#else   // HILMockNAS
+          []() { return Sensors::getInstance().state.kalman->getLastSample(); },
+#endif  // HILMockNAS
+          TRAJECTORY_SET, AirBrakesControllerConfig::ABK_CONFIG,
+          AirBrakesControllerConfig::ABK_CONFIG_INTERP,
+          [](float position) {
+              Actuators::getInstance().setServo(ServosList::AIR_BRAKES_SERVO,
+                                                position);
+          },
+          Main::dz)
+#endif  // INTERP
 {
     EventBroker::getInstance().subscribe(this, TOPIC_ABK);
     EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
@@ -225,10 +251,7 @@ void AirBrakesController::wiggleServo()
 {
     for (int i = 0; i < 2; i++)
     {
-        Actuators::getInstance().setServoAngle(AIR_BRAKES_SERVO,
-                                               ABK_SERVO_ROTATION);
-        miosix::Thread::sleep(500);
-        Actuators::getInstance().setServoAngle(AIR_BRAKES_SERVO, 0);
+        Actuators::getInstance().wiggleServo(AIR_BRAKES_SERVO);
         miosix::Thread::sleep(500);
     }
 }
diff --git a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h
index 0ad5f9a05cf8f02aba88bf932bf3a20c041c4841..cfb67a1862469fa7bb8842e2934ff1f1f85fd573 100644
--- a/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h
+++ b/src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.h
@@ -23,12 +23,17 @@
 #pragma once
 
 #include <Singleton.h>
-#include <algorithms/AirBrakes/AirBrakes.h>
 #include <diagnostic/PrintLogger.h>
 #include <events/FSM.h>
 
 #include "AirBrakesControllerData.h"
 
+#ifndef INTERP
+#include <algorithms/AirBrakes/AirBrakesPI.h>
+#else
+#include <algorithms/AirBrakes/AirBrakesInterp.h>
+#endif
+
 namespace Main
 {
 
@@ -60,8 +65,11 @@ private:
 
     void wiggleServo();
 
-    Boardcore::AirBrakes abk;
-
+#ifndef INTERP
+    Boardcore::AirBrakesPI abk;
+#else
+    Boardcore::AirBrakesInterp abk;
+#endif
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("main.abk");
 };
 
diff --git a/src/boards/Payload/Buses.h b/src/boards/Payload/Buses.h
index 4e1009b3d7333028f2696a3983b34fcac2fa4f7f..130f4a6ec464be99bd673ba095787d1e1533ef77 100644
--- a/src/boards/Payload/Buses.h
+++ b/src/boards/Payload/Buses.h
@@ -64,6 +64,8 @@ private:
           uart4(UART4, Boardcore::USARTInterface::Baudrate::B115200), spi1({}),
           spi2({})
     {
+        usart2.init();
+        usart3.init();
     }
 #endif
 };
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 7ab7a6a12ee4fd04af110869680695ae3b6ba605..4967a79c0e0e4e5661a9a716063fc389807dec51 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -48,11 +48,13 @@ using namespace Payload::RadioConfig;
 using namespace Common;
 
 // XBee interrupt
+#ifndef USE_SERIAL_TRANSCEIVER
 void __attribute__((used)) EXTI10_IRQHandlerImpl()
 {
-    if (Payload::Radio::getInstance().xbee != nullptr)
-        Payload::Radio::getInstance().xbee->handleATTNInterrupt();
+    if (Payload::Radio::getInstance().transceiver != nullptr)
+        Payload::Radio::getInstance().transceiver->handleATTNInterrupt();
 }
+#endif
 
 namespace Payload
 {
@@ -87,26 +89,31 @@ void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); }
 
 Radio::Radio()
 {
+#if defined(USE_SERIAL_TRANSCEIVER)
+    Boardcore::SerialTransceiver* transceiver;
+    transceiver = new SerialTransceiver(Buses::getInstance().usart2);
+#else
     // Create the SPI bus configuration
     SPIBusConfig config{};
     config.clockDivider = SPI::ClockDivider::DIV_16;
 
     // Create the xbee object
-    xbee = new Xbee::Xbee(
+    transceiver = new Xbee::Xbee(
         Buses::getInstance().spi2, config, miosix::xbee::cs::getPin(),
         miosix::xbee::attn::getPin(), miosix::xbee::reset::getPin());
-    xbee->setOnFrameReceivedListener(
+    transceiver->setOnFrameReceivedListener(
         bind(&Radio::onXbeeFrameReceived, this, _1));
 
-    Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT);
-
-    mavDriver =
-        new MavDriver(xbee, bind(&Radio::handleMavlinkMessage, this, _1, _2), 0,
-                      MAV_OUT_BUFFER_MAX_AGE);
+    Xbee::setDataRate(*transceiver, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT);
 
     enableExternalInterrupt(miosix::xbee::attn::getPin().getPort(),
                             miosix::xbee::attn::getPin().getNumber(),
                             InterruptTrigger::FALLING_EDGE);
+#endif
+
+    mavDriver = new MavDriver(transceiver,
+                              bind(&Radio::handleMavlinkMessage, this, _1, _2),
+                              0, MAV_OUT_BUFFER_MAX_AGE);
 
     // Add to the scheduler the periodic telemetries
     BoardScheduler::getInstance().getScheduler().addTask(
diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h
index f33e32eefada109e854eee62d90a8878ebbea07b..dbac47db8f12022960faabf8839622a41956605b 100644
--- a/src/boards/Payload/Radio/Radio.h
+++ b/src/boards/Payload/Radio/Radio.h
@@ -28,6 +28,14 @@
 #include <radio/Xbee/Xbee.h>
 #include <scheduler/TaskScheduler.h>
 
+#if defined(USE_SERIAL_TRANSCEIVER)
+#include <radio/SerialTransceiver/SerialTransceiver.h>
+#elif defined(USE_XBEE_TRANSCEIVER)
+#include <radio/Xbee/ATCommands.h>
+#else
+#include <radio/SX1278/SX1278.h>
+#endif
+
 namespace Payload
 {
 
@@ -40,7 +48,12 @@ class Radio : public Boardcore::Singleton<Radio>
     friend class Boardcore::Singleton<Radio>;
 
 public:
-    Boardcore::Xbee::Xbee* xbee;
+#if defined(USE_SERIAL_TRANSCEIVER)
+    Boardcore::SerialTransceiver* transceiver;
+#else
+    Boardcore::Xbee::Xbee* transceiver;
+#endif
+
     MavDriver* mavDriver;
 
     /**
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index b98e956ea87cfdccd6a1fd599a7ed4f82583b8bb..fc476a5892740fe3c2e4ae61a2e11a94b7ff3d44 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -69,27 +69,81 @@ BMX160Data Sensors::getBMX160LastSample()
 BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample()
 {
     miosix::PauseKernelLock lock;
+#ifndef HILSimulation
     return bmx160WithCorrection != nullptr
                ? bmx160WithCorrection->getLastSample()
                : BMX160WithCorrectionData{};
+#else
+    auto imuData = state.imu->getLastSample();
+    BMX160WithCorrectionData data;
+
+    data.accelerationTimestamp    = imuData.accelerationTimestamp;
+    data.accelerationX            = imuData.accelerationX;
+    data.accelerationY            = imuData.accelerationY;
+    data.accelerationZ            = imuData.accelerationZ;
+    data.angularVelocityTimestamp = imuData.angularVelocityTimestamp;
+    data.angularVelocityX         = imuData.angularVelocityX;
+    data.angularVelocityY         = imuData.angularVelocityY;
+    data.angularVelocityZ         = imuData.angularVelocityZ;
+    data.magneticFieldTimestamp   = imuData.magneticFieldTimestamp;
+    data.magneticFieldX           = imuData.magneticFieldX;
+    data.magneticFieldY           = imuData.magneticFieldY;
+    data.magneticFieldZ           = imuData.magneticFieldZ;
+
+    return data;
+#endif
 }
 
 LIS3MDLData Sensors::getMagnetometerLIS3MDLLastSample()
 {
     miosix::PauseKernelLock lock;
+#ifndef HILSimulation
     return lis3mdl != nullptr ? lis3mdl->getLastSample() : LIS3MDLData{};
+#else
+    return LIS3MDLData(state.magnetometer->getLastSample(),
+                       state.temperature->getLastSample());
+#endif
 }
 
 MS5803Data Sensors::getMS5803LastSample()
 {
     miosix::PauseKernelLock lock;
+
+#ifndef HILSimulation
     return ms5803 != nullptr ? ms5803->getLastSample() : MS5803Data{};
+#else
+    auto baroData = state.barometer->getLastSample();
+    auto tempData = state.temperature->getLastSample();
+
+    return MS5803Data(baroData.pressureTimestamp, baroData.pressure,
+                      tempData.temperatureTimestamp, tempData.temperature);
+#endif
 }
 
 UBXGPSData Sensors::getUbxGpsLastSample()
 {
     miosix::PauseKernelLock lock;
+#ifndef HILSimulation
     return ubxGps != nullptr ? ubxGps->getLastSample() : UBXGPSData{};
+#else
+    auto data = state.gps->getLastSample();
+    UBXGPSData ubxData;
+
+    ubxData.gpsTimestamp  = data.gpsTimestamp;
+    ubxData.latitude      = data.latitude;
+    ubxData.longitude     = data.longitude;
+    ubxData.height        = data.height;
+    ubxData.velocityNorth = data.velocityNorth;
+    ubxData.velocityEast  = data.velocityEast;
+    ubxData.velocityDown  = data.velocityDown;
+    ubxData.speed         = data.speed;
+    ubxData.track         = data.track;
+    ubxData.positionDOP   = data.positionDOP;
+    ubxData.satellites    = data.satellites;
+    ubxData.fix           = data.fix;
+
+    return ubxData;
+#endif
 }
 
 ADS1118Data Sensors::getADS1118LastSample()
@@ -115,14 +169,26 @@ SSCDANN030PAAData Sensors::getDplPressureLastSample()
 SSCDRRN015PDAData Sensors::getPitotPressureLastSample()
 {
     miosix::PauseKernelLock lock;
+#ifndef HILSimulation
     return pitotPressure != nullptr ? pitotPressure->getLastSample()
                                     : SSCDRRN015PDAData{};
+#else
+    auto pitotData = state.pitot->getLastSample();
+    SSCDRRN015PDAData data;
+    data.pressureTimestamp = pitotData.timestamp;
+    data.pressure          = pitotData.deltaP;
+    return data;
+#endif
 }
 
 PitotData Sensors::getPitotLastSample()
 {
     miosix::PauseKernelLock lock;
+#ifndef HILSimulation
     return pitot != nullptr ? pitot->getLastSample() : PitotData{};
+#else
+    return state.pitot->getLastSample();
+#endif
 }
 
 InternalADCData Sensors::getInternalADCLastSample()
@@ -216,6 +282,29 @@ Sensors::Sensors()
     bmx160Init();
     bmx160WithCorrectionInit();
 
+#ifdef HILSimulation
+    // Definition of the fake sensors for the simulation
+    state.accelerometer = new HILAccelerometer(N_DATA_ACCEL);
+    state.barometer     = new HILBarometer(N_DATA_BARO);
+    state.pitot         = new HILPitot(N_DATA_PITOT);
+    state.gps           = new HILGps(N_DATA_GPS);
+    state.gyro          = new HILGyroscope(N_DATA_GYRO);
+    state.magnetometer  = new HILMagnetometer(N_DATA_MAGN);
+    state.imu           = new HILImu(N_DATA_IMU);
+    state.temperature   = new HILTemperature(N_DATA_TEMP);
+    state.kalman        = new HILKalman(N_DATA_KALM);
+
+    sensorsMap = {{state.accelerometer, accelConfig},
+                  {state.barometer, baroConfig},
+                  {state.pitot, pitotConfig},
+                  {state.magnetometer, magnConfig},
+                  {state.imu, imuConfig},
+                  {state.gps, gpsConfig},
+                  {state.gyro, gyroConfig},
+                  {state.temperature, tempConfig},
+                  {state.kalman, kalmConfig}};
+#endif
+
     // Create the sensor manager
     sensorManager = new SensorManager(sensorsMap);
 }
@@ -230,6 +319,21 @@ Sensors::~Sensors()
     delete staticPressure;
     delete dplPressure;
     delete pitotPressure;
+
+#ifdef HILSimulation
+    delete state.accelerometer;
+    delete state.barometer;
+    delete state.pitot;
+    delete state.gps;
+    delete state.gyro;
+    delete state.magnetometer;
+    delete state.imu;
+    delete state.temperature;
+    delete state.kalman;
+#endif
+
+    sensorManager->stop();
+    delete sensorManager;
 }
 
 void Sensors::bmx160Init()
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 07c1c8e12dde695a8af8801cf85b24058eeac0d3..ce0f67f9f3e0ec1768fa41b2a78d27ebbb9fd6a7 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -37,6 +37,10 @@
 #include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h>
 #include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h>
 #include <sensors/analog/pressure/nxp/MPXHZ6130A.h>
+#ifdef HILSimulation
+#include <HIL_algorithms/HILMockKalman.h>
+#include <HIL_sensors/HILSensors.h>
+#endif  // HILSimulation
 
 namespace Payload
 {
@@ -50,6 +54,25 @@ public:
 
     bool isStarted();
 
+#ifdef HILSimulation
+public:
+    /**
+     * structure that contains all the sensors used in the simulation
+     */
+    struct StateComplete
+    {
+        HILAccelerometer* accelerometer;
+        HILBarometer* barometer;
+        HILPitot* pitot;
+        HILGps* gps;
+        HILGyroscope* gyro;
+        HILMagnetometer* magnetometer;
+        HILTemperature* temperature;
+        HILImu* imu;
+        HILKalman* kalman;
+    } state;
+#endif  // HILSimulation
+
     Boardcore::BMX160* bmx160;
 
     Boardcore::BMX160Data getBMX160LastSample();
diff --git a/src/entrypoints/Main/main-entry-maker-faire.cpp b/src/entrypoints/Main/main-entry-maker-faire.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c594a7789a25c766e623fcf730e043ccd0e2bc7
--- /dev/null
+++ b/src/entrypoints/Main/main-entry-maker-faire.cpp
@@ -0,0 +1,215 @@
+/* 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.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/BoardScheduler.h>
+#include <Main/CanHandler/CanHandler.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/AirBrakesController/AirBrakesController.h>
+#include <Main/StateMachines/Deployment/Deployment.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <common/events/Events.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+#include <miosix.h>
+#include <utils/PinObserver/PinObserver.h>
+
+#include "kernel/scheduler/priority/priority_scheduler.h"
+
+#ifdef HILSimulation
+#include <HIL.h>
+#include <HIL_algorithms/HILMockAerobrakeAlgorithm.h>
+#include <HIL_algorithms/HILMockKalman.h>
+#include <HIL_sensors/HILSensors.h>
+#endif
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Main;
+using namespace Common;
+
+int main()
+{
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+
+#ifdef HILSimulation
+    auto flightPhasesManager = HIL::getInstance().flightPhasesManager;
+
+    flightPhasesManager->setCurrentPositionSource(
+        []()
+        {
+            return TimedTrajectoryPoint{
+                Main::NASController::getInstance().getNasState()};
+        });
+
+    HIL::getInstance().start();
+
+    BoardScheduler::getInstance().getScheduler().addTask(
+        []() { Actuators::getInstance().sendToSimulator(); }, 100);
+#endif
+
+    if (!Logger::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting SD logger");
+    }
+
+    if (!EventBroker::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the EventBroker");
+    }
+
+    // Start the radio
+    if (!Radio::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the radio");
+    }
+
+    // Start the can interface
+    // if (!CanHandler::getInstance().start())
+    // {
+    //     initResult = false;
+    //     LOG_ERR(logger, "Error starting the CAN interface");
+    // }
+
+    // Start the state machines
+#ifdef HILSimulation
+    ADAController::getInstance().setUpdateDataFunction(
+        [](Boardcore::ADAState state)
+        { HIL::getInstance().getElaboratedData()->addADAState(state); });
+#endif
+    if (!ADAController::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ADAController");
+    }
+    if (!AirBrakesController::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the AirBrakesController");
+    }
+    if (!Deployment::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Deployment");
+    }
+    if (!FlightModeManager::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the FlightModeManager");
+    }
+#ifdef HILSimulation
+    NASController::getInstance().setUpdateDataFunction(
+        [](Boardcore::NASState state)
+        { HIL::getInstance().getElaboratedData()->addNASState(state); });
+#endif
+    if (!NASController::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the NASController");
+    }
+
+    // Start the sensors sampling
+    if (!Sensors::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors");
+    }
+
+    // Start the pin handler and observer
+    if (!PinObserver::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the PinObserver");
+    }
+
+    // Start the board task scheduler
+    if (!BoardScheduler::getInstance().getScheduler().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the BoardScheduler");
+    }
+
+    // If all is correctly set up i publish the init ok
+    if (initResult)
+        EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+    else
+        EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+
+    // Log all events
+    EventSniffer sniffer(
+        EventBroker::getInstance(), TOPICS_LIST,
+        [](uint8_t event, uint8_t topic)
+        {
+            EventData ev{TimestampTimer::getTimestamp(), event, topic};
+            Logger::getInstance().log(ev);
+        });
+
+    // Set the clock divider for the analog circuitry (/8)
+    ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1;
+    InternalADC internalADC = InternalADC(ADC3, 3.3);
+    internalADC.enableChannel(InternalADC::CH5);
+    internalADC.init();
+
+    std::function<ADCData()> get_batVoltage_function =
+        std::bind(&InternalADC::getVoltage, &internalADC, InternalADC::CH5);
+
+    BatteryVoltageSensor batterySensor(get_batVoltage_function, 5.98);
+
+    bool servoEnabled = true;
+
+    // Periodical statistics
+    while (true)
+    {
+        // checking for battery charge. if too low for the actuator (< 10.5 V),
+        // disable the real actuation of the servo
+        internalADC.sample();
+        batterySensor.sample();
+        float vbat = batterySensor.getLastSample().batVoltage -
+                     0.4;  // subtracting 0.4 as offset
+
+        printf("vbat: %f\n", vbat);
+
+        if (servoEnabled && vbat < 10.5)
+        {
+            printf("*** AIRBRAKES SERVO DISABLED ***");
+            servoEnabled = false;
+            Actuators::getInstance().disableServo(AIR_BRAKES_SERVO);
+        }
+
+        Thread::sleep(1000);
+        Logger::getInstance().log(CpuMeter::getCpuStats());
+        CpuMeter::resetCpuStats();
+        Logger::getInstance().logStats();
+        Radio::getInstance().logStatus();
+    }
+}
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index cac0120858542a519887e34f3a43b46d26a15951..44b2f9dab57f4b9e6b3834e3d10527ccab4fd959 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -42,6 +42,13 @@
 #include <events/utils/EventSniffer.h>
 #include <miosix.h>
 
+#ifdef HILSimulation
+#include <HIL.h>
+#include <HIL_algorithms/HILMockAerobrakeAlgorithm.h>
+#include <HIL_algorithms/HILMockKalman.h>
+#include <HIL_sensors/HILSensors.h>
+#endif
+
 using namespace miosix;
 using namespace Boardcore;
 using namespace Payload;
@@ -52,6 +59,23 @@ int main()
     bool initResult    = true;
     PrintLogger logger = Logging::getLogger("main");
 
+#ifdef HILSimulation
+    auto flightPhasesManager = HIL::getInstance().flightPhasesManager;
+
+    flightPhasesManager->setCurrentPositionSource(
+        []() {
+            return TimedTrajectoryPoint{
+                NASController::getInstance().getNasState()};
+        });
+
+    HIL::getInstance().start();
+
+    BoardScheduler::getInstance().getScheduler().addTask(
+        []() { HIL::getInstance().send(0.0f); }, 100);
+
+    // flightPhasesManager->registerToFlightPhase(FlightPhases::FLYING, )
+#endif
+
     if (!Logger::getInstance().start())
     {
         initResult = false;
diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
index dacffb383f4ff9f6819519a354e96b11aa728b1a..8bcadd31aa1090b9f343d9bea4085761d19089bc 100644
--- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
+++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
@@ -33,21 +33,23 @@ using namespace Common;
 
 HILFlightPhasesManager::HILFlightPhasesManager()
     : Boardcore::EventHandler(),
-      flagsFlightPhases({{FlightPhases::SIMULATION_STARTED, false},
+      flagsFlightPhases({{FlightPhases::SIM_FLYING, false},
+                         {FlightPhases::SIM_ASCENT, false},
+                         {FlightPhases::SIM_BURNING, false},
+                         {FlightPhases::SIM_AEROBRAKES, false},
+                         {FlightPhases::SIM_PARA1, false},
+                         {FlightPhases::SIM_PARA2, false},
+                         {FlightPhases::SIMULATION_STARTED, false},
                          {FlightPhases::CALIBRATION, false},
                          {FlightPhases::CALIBRATION_OK, false},
                          {FlightPhases::ARMED, false},
                          {FlightPhases::LIFTOFF_PIN_DETACHED, false},
-                         {FlightPhases::FLYING, false},
-                         {FlightPhases::ASCENT, false},
-                         {FlightPhases::BURNING, false},
+                         {FlightPhases::LIFTOFF, false},
                          {FlightPhases::AEROBRAKES, false},
-                         {FlightPhases::SIM_AEROBRAKES, false},
                          {FlightPhases::APOGEE, false},
                          {FlightPhases::PARA1, false},
                          {FlightPhases::PARA2, false},
-                         {FlightPhases::SIMULATION_STOPPED, false}}),
-      last_event(0)
+                         {FlightPhases::SIMULATION_STOPPED, false}})
 {
     prev_flagsFlightPhases = flagsFlightPhases;
 
@@ -84,7 +86,7 @@ void HILFlightPhasesManager::setFlagFlightPhase(FlightPhases flag,
 
 void HILFlightPhasesManager::processFlags(FlightPhasesFlags hil_flags)
 {
-    updateFlags(hil_flags);
+    updateSimulatorFlags(hil_flags);
 
     std::vector<FlightPhases> changed_flags;
 
@@ -97,35 +99,36 @@ void HILFlightPhasesManager::processFlags(FlightPhasesFlags hil_flags)
         changed_flags.push_back(FlightPhases::SIMULATION_STARTED);
     }
 
-    if (flagsFlightPhases[FlightPhases::FLYING])
+    if (flagsFlightPhases[FlightPhases::SIM_FLYING])
     {
-        if (isSetTrue(FlightPhases::FLYING))
+        if (isSetTrue(FlightPhases::SIM_FLYING))
         {
             TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
             sEventBroker.post(FLIGHT_LAUNCH_PIN_DETACHED, TOPIC_FLIGHT);
+            changed_flags.push_back(FlightPhases::SIM_FLYING);
         }
-        if (isSetFalse(FlightPhases::BURNING))
+        if (isSetFalse(FlightPhases::SIM_BURNING))
         {
-            registerOutcomes(FlightPhases::BURNING);
+            registerOutcomes(FlightPhases::SIM_BURNING);
             TRACE("[HIL] ------- STOPPED BURNING ! ------- \n");
-            changed_flags.push_back(FlightPhases::BURNING);
+            changed_flags.push_back(FlightPhases::SIM_BURNING);
         }
         if (isSetTrue(FlightPhases::SIM_AEROBRAKES))
         {
             registerOutcomes(FlightPhases::SIM_AEROBRAKES);
             changed_flags.push_back(FlightPhases::SIM_AEROBRAKES);
         }
-        if (isSetTrue(FlightPhases::PARA1))
+        if (isSetTrue(FlightPhases::SIM_PARA1))
         {
-            registerOutcomes(FlightPhases::PARA1);
+            registerOutcomes(FlightPhases::SIM_PARA1);
             TRACE("[HIL] ------- PARACHUTE 1 ! ------- \n");
-            changed_flags.push_back(FlightPhases::PARA1);
+            changed_flags.push_back(FlightPhases::SIM_PARA1);
         }
-        if (isSetTrue(FlightPhases::PARA2))
+        if (isSetTrue(FlightPhases::SIM_PARA2))
         {
-            registerOutcomes(FlightPhases::PARA2);
+            registerOutcomes(FlightPhases::SIM_PARA2);
             TRACE("[HIL] ------- PARACHUTE 2 ! ------- \n");
-            changed_flags.push_back(FlightPhases::PARA2);
+            changed_flags.push_back(FlightPhases::SIM_PARA2);
         }
     }
 
@@ -155,7 +158,7 @@ void HILFlightPhasesManager::printOutcomes()
            (double)(t_stop - t_start) / 1000000.0f);
 
     printf("Motor stopped burning (simulation flag): \n");
-    outcomes[FlightPhases::BURNING].print(t_liftoff);
+    outcomes[FlightPhases::SIM_BURNING].print(t_liftoff);
 
     printf("Airbrakes exit shadowmode: \n");
     outcomes[FlightPhases::AEROBRAKES].print(t_liftoff);
@@ -183,20 +186,17 @@ void HILFlightPhasesManager::printOutcomes()
  * @brief Updates the flags of the object with the flags sent from matlab
  * and checks for the apogee
  */
-void HILFlightPhasesManager::updateFlags(FlightPhasesFlags hil_flags)
+void HILFlightPhasesManager::updateSimulatorFlags(FlightPhasesFlags hil_flags)
 {
-    // TODO: Add ifdefs in order to select which flags to take from matlab
-    flagsFlightPhases[FlightPhases::ASCENT]  = hil_flags.flag_ascent;
-    flagsFlightPhases[FlightPhases::FLYING]  = hil_flags.flag_flight;
-    flagsFlightPhases[FlightPhases::BURNING] = hil_flags.flag_burning;
-
-    // Flags PARA1, PARA2 and SIM_AEROBRAKES ignored from matlab
-    // flagsFlightPhases[SIM_AEROBRAKES] = hil_flags.flag_airbrakes;
-    // flagsFlightPhases[PARA1]          = hil_flags.flag_para1;
-    // flagsFlightPhases[PARA2]          = hil_flags.flag_para2;
+    flagsFlightPhases[FlightPhases::SIM_ASCENT]     = hil_flags.flag_ascent;
+    flagsFlightPhases[FlightPhases::SIM_FLYING]     = hil_flags.flag_flight;
+    flagsFlightPhases[FlightPhases::SIM_BURNING]    = hil_flags.flag_burning;
+    flagsFlightPhases[FlightPhases::SIM_AEROBRAKES] = hil_flags.flag_airbrakes;
+    flagsFlightPhases[FlightPhases::SIM_PARA1]      = hil_flags.flag_para1;
+    flagsFlightPhases[FlightPhases::SIM_PARA2]      = hil_flags.flag_para2;
 
     flagsFlightPhases[FlightPhases::SIMULATION_STOPPED] =
-        isSetFalse(FlightPhases::FLYING) ||
+        isSetFalse(FlightPhases::SIM_FLYING) ||
         prev_flagsFlightPhases[FlightPhases::SIMULATION_STOPPED];
 }
 
@@ -233,13 +233,13 @@ void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e)
             t_liftoff = Boardcore::TimestampTimer::getTimestamp();
             TRACE("[HIL] ------- LIFTOFF -------: %f, %f \n",
                   getCurrentPosition().z, getCurrentPosition().vz);
-            changed_flags.push_back(FlightPhases::FLYING);
-            changed_flags.push_back(FlightPhases::ASCENT);
+            changed_flags.push_back(FlightPhases::LIFTOFF);
             break;
         case ABK_SHADOW_MODE_TIMEOUT:
             setFlagFlightPhase(FlightPhases::AEROBRAKES, true);
             registerOutcomes(FlightPhases::AEROBRAKES);
-            TRACE("[HIL] ------- AEROBRAKES ENABLED ! ------- \n");
+            TRACE("[HIL] ABK shadow mode timeout\n");
+            // TRACE("[HIL] ------- AEROBRAKES ENABLED ! ------- \n");
             changed_flags.push_back(FlightPhases::AEROBRAKES);
             break;
         case ADA_SHADOW_MODE_TIMEOUT:
@@ -298,8 +298,6 @@ void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e)
     }
 
     prev_flagsFlightPhases = flagsFlightPhases;
-
-    last_event = e;
 }
 
 bool HILFlightPhasesManager::isSetTrue(FlightPhases phase)
diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
index c18d26b002e9b6eb07916e6579efb747df5e4e3d..44c53b8e86d4415ac509f5a33259534f346d9524 100644
--- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
+++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
@@ -41,16 +41,22 @@ class HILTransceiver;
 
 enum class FlightPhases
 {
+    // simulator flags
+    SIM_FLYING,
+    SIM_ASCENT,
+    SIM_BURNING,
+    SIM_AEROBRAKES,
+    SIM_PARA1,
+    SIM_PARA2,
+
+    // flight flags
     SIMULATION_STARTED,
     CALIBRATION,
     CALIBRATION_OK,
     ARMED,
     LIFTOFF_PIN_DETACHED,
-    FLYING,
-    ASCENT,
-    BURNING,
+    LIFTOFF,
     AEROBRAKES,
-    SIM_AEROBRAKES,
     APOGEE,
     PARA1,
     PARA2,
@@ -112,7 +118,7 @@ private:
      * @brief Updates the flags of the object with the flags sent from matlab
      * and checks for the apogee
      */
-    void updateFlags(FlightPhasesFlags hil_flags);
+    void updateSimulatorFlags(FlightPhasesFlags hil_flags);
 
     bool isSetTrue(FlightPhases phase);
 
@@ -126,6 +132,4 @@ private:
     std::map<FlightPhases, vector<TCallback>> callbacks;
     std::map<FlightPhases, Outcomes> outcomes;
     std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition;
-
-    uint8_t last_event;
 };
diff --git a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
index c59c16dc9f94e575e445e994d910ed897c6c5d58..f5e23848123a92fadb8c29974db5fc6abe64eaa2 100644
--- a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
+++ b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
@@ -20,15 +20,21 @@
  * THE SOFTWARE.
  */
 
-#include <Main/Buses.h>
-
 #include "HIL.h"
 
+#ifdef PAYLOAD_ENTRY
+#include <Payload/Buses.h>
+using namespace Payload;
+#else
+#include <Main/Buses.h>
+using namespace Main;
+#endif
+
 /**
  * @brief Construct a serial connection attached to a control algorithm
  */
 HILTransceiver::HILTransceiver()
-    : hilSerial(Main::Buses::getInstance().usart3), actuatorData(ActuatorData{})
+    : hilSerial(Buses::getInstance().uart4), actuatorData(ActuatorData{})
 {
 }
 
diff --git a/src/hardware_in_the_loop/HILConfig.h b/src/hardware_in_the_loop/HILConfig.h
index b3f70973efdcaa294950806e5c498e69a9325815..18834996c2fc9413487ad8d57a3e477e7e9b7e28 100644
--- a/src/hardware_in_the_loop/HILConfig.h
+++ b/src/hardware_in_the_loop/HILConfig.h
@@ -40,21 +40,8 @@
  */
 
 /* Hardware in the loop entrypoint */
-#if defined(HARDWARE_IN_THE_LOOP)
-#include "entrypoints/hardware_in_the_loop/HILSimulationConfig.h"
-/* serial simulation with sample manager */
-#elif defined(HILSimulation)
+#if defined(HILSimulation)
 #include <HILSimulationConfig.h>
-#elif defined(HIL_ADA)
-#include "test-HIL+ADA/HILSimulationConfig.h"
-#elif defined(HIL_AEROBRAKE)
-#include "test-HIL+Airbrake/HILSimulationConfig.h"
-#elif defined(HIL_ADA_AEROBRAKE)
-#include "test-HIL+ADA+Airbrake/HILSimulationConfig.h"
-#elif defined(HIL_ADA_AEROBRAKECONTROLLER)
-#include "test-HIL+ADA+AirbrakeController/HILSimulationConfig.h"
-#elif defined(HIL_ADA_AEROBRAKECONTROLLER_NAS)
-#include "test-HIL+ADA+AirbrakeController+nas/HILSimulationConfig.h"
 /*
 #elif defined(HIL_<tuoFlag>)
 #include "<test-directory>/HILSimulationConfig.h"
diff --git a/src/hardware_in_the_loop/HIL_actuators/HILServo.h b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
index 8ab6b707288d02a1911bd1c82ba7695bed8f821a..947bd8ae87e0f30d7b69306ebfc6087dfafd87bd 100644
--- a/src/hardware_in_the_loop/HIL_actuators/HILServo.h
+++ b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
@@ -25,26 +25,17 @@
 #include <actuators/Servo/Servo.h>
 
 #include "HIL.h"
-#include "HILConfig.h"
 
 class HILServo : public Boardcore::Servo
 {
 public:
     explicit HILServo(TIM_TypeDef* const timer,
                       Boardcore::TimerUtils::Channel pwmChannel,
-                      unsigned int minPulse = 1000,
-                      unsigned int maxPulse = 2000, unsigned int frequency = 50)
+                      unsigned int minPulse, unsigned int maxPulse,
+                      unsigned int frequency = 50)
         : Servo(timer, pwmChannel, minPulse, maxPulse, frequency)
     {
     }
 
-    void setPosition(float position, bool limited = true)
-    {
-        Servo::setPosition(position, limited);
-
-        // Send the position to MatLab
-        HIL::getInstance().send(position);
-    }
-
     void sendToSimulator() { HIL::getInstance().send(getPosition()); }
 };
diff --git a/src/tests/Main/actuators/test-airbrakes-st.cpp b/src/tests/Main/actuators/test-airbrakes-st.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4650121baec6bb9dd210c62d7f626de9fa5e271f
--- /dev/null
+++ b/src/tests/Main/actuators/test-airbrakes-st.cpp
@@ -0,0 +1,86 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, 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.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/Sensors/Sensors.h>
+#include <actuators/Servo/Servo.h>
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/ClockUtils.h>
+
+#include <iostream>
+
+#include "HIL_actuators/HILServo.h"
+
+using namespace Boardcore;
+using namespace miosix;
+
+HILServo airbrakesServo(TIM8, TimerUtils::Channel::CHANNEL_2, 1080, 1600);
+
+// Position to cycle through for the servo 1, 2 and 3
+
+void moveServo()
+{
+    airbrakesServo.setPosition(1);
+    Thread::sleep(1000);
+    airbrakesServo.setPosition(0);
+}
+
+int main()
+{
+    char c[10];
+
+    // Set the clock divider for the analog circuitry (/8)
+    ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1;
+    InternalADC internalADC = InternalADC(ADC3, 3.3);
+    internalADC.enableChannel(InternalADC::CH5);
+    internalADC.init();
+
+    std::function<ADCData()> get_batVoltage_function =
+        std::bind(&InternalADC::getVoltage, &internalADC, InternalADC::CH5);
+
+    BatteryVoltageSensor batterySensor(get_batVoltage_function, 5.98);
+
+    // Enable the timers
+    airbrakesServo.enable();
+    airbrakesServo.setPosition(0);
+
+    while (true)
+    {
+        // checking for battery charge. if too low for the actuator (< 10.5 V),
+        // disable the real actuation of the servo
+        internalADC.sample();
+        batterySensor.sample();
+        float vbat = batterySensor.getLastSample().batVoltage -
+                     0.4;  // subtracting 0.4 as offset
+
+        if (vbat < 10.5)
+        {
+            airbrakesServo.disable();
+            printf("Airbrakes can't be attuated, vbat too low\n");
+        }
+
+        scanf("%c\n", c);
+        moveServo();
+        printf("vbat: %f\n", vbat);
+    }
+}