diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ffb1eac6b548a07e8f1fbd63e337efd529a10a1c
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,404 @@
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Author: Damiano Amatruda
+#
+# 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.
+
+cmake_minimum_required(VERSION 3.16)
+include(skyward-boardcore/cmake/sbs.cmake)
+
+#-----------------------------------------------------------------------------#
+#                                   Project                                   #
+#-----------------------------------------------------------------------------#
+
+project(SkywardOnBoardSoftware)
+
+#-----------------------------------------------------------------------------#
+#                                 Entrypoints                                 #
+#-----------------------------------------------------------------------------#
+
+add_executable(death-stack-x-entry
+    src/entrypoints/death-stack-x-entry.cpp
+    ${DEATHSTACK_NEW_SOURCES}
+    ${MOCK_SENSORS_DATA_SOURCES}
+)
+target_include_directories(death-stack-x-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(death-stack-x-entry PRIVATE FLIGHT EUROC)
+sbs_target(death-stack-x-entry stm32f429zi_skyward_death_stack_x)
+
+add_executable(death-stack-x-testsuite src/entrypoints/death-stack-x-testsuite.cpp ${DEATHSTACK_NEW_SOURCES})
+target_include_directories(death-stack-x-testsuite PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(death-stack-x-testsuite stm32f429zi_skyward_death_stack_x)
+
+#add_executable(death-stack-entry src/entrypoints/death-stack-entry.cpp ${DEATHSTACK_SOURCES})
+#target_include_directories(death-stack-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(death-stack-entry PRIVATE DEATH_STACK_1)
+#sbs_target(death-stack-entry stm32f429zi_skyward_death_stack_x)
+
+#add_executable(death-stack-testsuite src/entrypoints/death-stack-testsuite.cpp ${DEATHSTACK_SOURCES})
+#target_include_directories(death-stack-testsuite PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(death-stack-testsuite PRIVATE DEATH_STACK_1)
+#sbs_target(death-stack-testsuite stm32f429zi_skyward_death_stack_x)
+
+#add_executable(windtunnel-entry src/entrypoints/windtunnel-entry.cpp ${DEATHSTACK_NEW_SOURCES})
+#target_include_directories(windtunnel-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(windtunnel-entry stm32f429zi_skyward_death_stack_x)
+
+#add_executable(hil-entry
+#    src/entrypoints/hardware_in_the_loop/hil-entry.cpp
+#    ${FMM_SOURCES}
+#    ${PINHANDLER_SOURCES}
+#    ${ADA_SOURCES}
+#    ${NAVIGATION_SYSTEM_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#    ${DEPLOYMENT_SOURCES}
+#)
+#target_include_directories(hil-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(hil-entry PRIVATE HARDWARE_IN_THE_LOOP)
+#sbs_target(hil-entry stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#                                Tests - Catch                                #
+#-----------------------------------------------------------------------------#
+
+#add_executable(catch-tests-entry
+#    src/tests/catch/catch-tests-entry.cpp
+#    ${DEATHSTACK_SOURCES}
+#    ${HERMES_TESTS_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#    ${MOCK_SENSORS_DATA_SOURCES}
+#)
+#target_include_directories(catch-tests-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(catch-tests-entry PRIVATE DEATH_STACK_1)
+#sbs_target(catch-tests-entry stm32f429zi_skyward_death_stack_x)
+
+add_executable(tests-catch
+    src/tests/catch/catch-tests-entry.cpp
+    ${DEATHSTACK_NEW_SOURCES}
+    ${TESTS_OBSW_SOURCES}
+    ${ADA_TEST_SOURCES}
+    ${MOCK_SENSORS_DATA_SOURCES}
+)
+target_include_directories(tests-catch PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(tests-catch stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#                               Tests - Drivers                               #
+#-----------------------------------------------------------------------------#
+
+#add_executable(test-imus src/tests/drivers/test-imus.cpp)
+#target_include_directories(test-imus PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-imus PRIVATE DEATH_STACK_1)
+#sbs_target(test-imus stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-cutter src/tests/drivers/test-cutter.cpp)
+#target_include_directories(test-cutter PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-cutter stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-mavlink src/tests/drivers/test-mavlink.cpp ${TMTC_SOURCES} ${LOGSERVICE_SOURCES})
+#target_include_directories(test-mavlink PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-mavlink PRIVATE TRACE_EVENTS DEATH_STACK_1)
+#sbs_target(test-mavlink stm32f429zi_skyward_death_stack_x)
+
+#add_executable(ledwave src/tests/ledwave.cpp)
+#target_include_directories(ledwave PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(ledwave PRIVATE <compile>)
+#sbs_target(ledwave stm32f429zi_skyward_death_stack_x)
+
+add_executable(ramtest src/tests/ram_test/ramtest.cpp ${RAM_TEST_SOURCES})
+target_include_directories(ramtest PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(ramtest PRIVATE __ENABLE_XRAM)
+sbs_target(ramtest stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-motor src/tests/drivers/test-motor.cpp ${DEPLOYMENT_SOURCES})
+#target_include_directories(test-motor PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-motor stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-servo src/tests/drivers/test-servo.cpp ${LOGSERVICE_SOURCES} ${AIRBRAKES_SOURCES})
+#target_include_directories(test-servo PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-servo stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-hse src/tests/test-hse.cpp)
+#target_include_directories(test-hse PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-hse stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-all-sensors src/tests/drivers/test-all-sensors.cpp)
+#target_include_directories(test-all-sensors PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-all-sensors PRIVATE DEATH_STACK_1)
+#sbs_target(test-all-sensors stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-preassure-calib src/tests/drivers/test-pressure-calib.cpp)
+#target_include_directories(test-preassure-calib PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-preassure-calib PRIVATE DEATH_STACK_1)
+#sbs_target(test-preassure-calib stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#                             Tests - Components                              #
+#-----------------------------------------------------------------------------#
+
+#add_executable(test-canproxy src/tests/test-canproxy.cpp)
+#target_include_directories(test-canproxy PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-canproxy stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-pinhandler src/tests/test-pinhandler.cpp ${PINHANDLER_SOURCES} ${LOGSERVICE_SOURCES})
+target_include_directories(test-pinhandler PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(test-pinhandler stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-logproxy src/tests/test-logproxy.cpp ${LOGSERVICE_SOURCES})
+#target_include_directories(test-logproxy PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-logproxy PRIVATE DEATH_STACK_1)
+#sbs_target(test-logproxy stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-sensormanager src/tests/test-sensormanager.cpp ${LOGSERVICE_SOURCES} ${ADA_TEST_SOURCES})
+#target_include_directories(test-sensormanager PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-sensormanager PRIVATE USE_MOCK_SENSORS DEATH_STACK_1)
+#sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
+
+# Cannot use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class
+#add_executable(
+#    test-sm+tmtc src/tests/test-sm+tmtc.cpp
+#    ${LOGSERVICE_SOURCES}
+#    ${ADA_SOURCES}
+#    ${TMTC_SOURCES}
+#    ${EVT_FUNCTIONS_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#)
+#target_include_directories(test-sm+tmtc PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-sm+tmtc PRIVATE DEATH_STACK_1)
+#sbs_target(test-sm+tmtc stm32f429zi_skyward_death_stack)
+
+#add_executable(test-fmm src/tests/catch/fsm/test-fmm.cpp ${DEATHSTACK_NEW_SOURCES})
+#target_include_directories(test-fmm PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-fmm PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-fmm stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-fmm-interactive src/tests/test-fmm-interactive.cpp ${DEATHSTACK_NEW_SOURCES})
+#target_include_directories(test-fmm-interactive PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-fmm-interactive stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#                                 Tests - FSM                                 #
+#-----------------------------------------------------------------------------#
+
+#add_executable(test-ada
+#    src/tests/catch/fsm/test-ada.cpp
+#    ${ADA_SOURCES}
+#    ${TEST_UTILS_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#    ${MOCK_SENSORS_DATA_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-ada PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-ada PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-ada stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-ada-simulation
+#    src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+#    ${ADA_TEST_SOURCES}
+#    ${DEATHSTACK_NEW_SOURCES}
+#)
+#target_include_directories(test-ada-simulation PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-ada-simulation PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-ada-simulation stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-ada-dpl-simulation
+    src/tests/test-ada-dpl-simulation.cpp
+    ${ADA_TEST_SOURCES}
+    ${DEATHSTACK_NEW_SOURCES}
+)
+target_include_directories(test-ada-dpl-simulation PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(test-ada-dpl-simulation stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-nas
+    src/tests/catch/fsm/test-nas.cpp
+    ${NAVIGATION_SYSTEM_SOURCES}
+    ${TEST_UTILS_SOURCES}
+    ${MOCK_SENSORS_DATA_SOURCES}
+    ${LOGSERVICE_SOURCES}
+)
+target_include_directories(test-nas PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(test-nas PRIVATE STANDALONE_CATCH1_TEST)
+sbs_target(test-nas stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-nas-simulation
+    src/tests/catch/nas/test-nas-simulation.cpp
+    ${NAVIGATION_SYSTEM_SOURCES}
+    ${MOCK_SENSORS_DATA_SOURCES}
+    ${LOGSERVICE_SOURCES}
+)
+target_include_directories(test-nas-simulation PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(test-nas-simulation PRIVATE STANDALONE_CATCH1_TEST)
+sbs_target(test-nas-simulation stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-tmtc src/tests/test-tmtc.cpp ${TEST_UTILS_SOURCES} ${DEATHSTACK_NEW_SOURCES})
+#target_include_directories(test-tmtc PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-tmtc PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-tmtc stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-flightstatsrecorder
+#    src/tests/catch/fsm/test-flightstatsrecorder.cpp
+#    ${TEST_UTILS_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-flightstatsrecorder PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-flightstatsrecorder PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-flightstatsrecorder stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-ignition src/tests/catch/fsm/test-ignition.cpp ${TEST_UTILS_SOURCES})
+#target_include_directories(test-ignition PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-ignition PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-ignition stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#                                 Tests - HIL                                 #
+#-----------------------------------------------------------------------------#
+
+#add_executable(test-SerialInterface src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp)
+#target_include_directories(test-SerialInterface PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-SerialInterface PRIVATE HIL_SERIALINTERFACE)
+#sbs_target(test-SerialInterface stm32f407vg_stm32f4discovery)
+
+#add_executable(test-HIL src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp)
+#target_include_directories(test-HIL PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL PRIVATE HIL)
+#sbs_target(test-HIL stm32f407vg_stm32f4discovery)
+
+#add_executable(test-HIL+Aerobrake src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp)
+#target_include_directories(test-HIL+Aerobrake PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL+Aerobrake PRIVATE HIL_AEROBRAKE)
+#sbs_target(test-HIL+Aerobrake stm32f407vg_stm32f4discovery)
+
+#add_executable(test-HIL+AerobrakeController src/tests/hardware_in_the_loop/test-HIL+AerobrakeController/test-HIL+AerobrakeController.cpp)
+#target_include_directories(test-HIL+AerobrakeController PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL+AerobrakeController PRIVATE HIL_AEROBRAKE)
+#sbs_target(test-HIL+AerobrakeController stm32f407vg_stm32f4discovery)
+
+#add_executable(test-HIL+ADA
+#    src/tests/hardware_in_the_loop/test-HIL+ApogeeDetectionAlgorithm/test-HIL+ADA.cpp
+#    ${ADA_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-HIL+ADA PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL+ADA PRIVATE HIL_ADA)
+#sbs_target(test-HIL+ADA stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-HIL+ADA+Aerobrake
+#    src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp
+#    ${ADA_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-HIL+ADA+Aerobrake PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL+ADA+Aerobrake PRIVATE HIL_ADA_AEROBRAKE)
+#sbs_target(test-HIL+ADA+Aerobrake stm32f407vg_stm32f4discovery)
+
+#add_executable(test-HIL+ADA+AerobrakeController+nas
+#    src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp
+#    ${ADA_SOURCES}
+#    ${ADA_TEST_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#    ${NAVIGATION_SYSTEM_SOURCES}
+#)
+#target_include_directories(test-HIL+ADA+AerobrakeController+nas PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-HIL+ADA+AerobrakeController+nas PRIVATE HIL_ADA_AEROBRAKECONTROLLER_NAS)
+#sbs_target(test-HIL+ADA+AerobrakeController+nas stm32f429zi_stm32f4discovery)
+
+#add_executable(eigen-test src/tests/eigen-test.cpp)
+#target_include_directories(eigen-test PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(eigen-test stm32f429zi_stm32f4discovery)
+
+#add_executable(test-mock-sensors src/tests/mock_sensors/test-mock-sensors.cpp ${MOCK_SENSORS_DATA})
+#target_include_directories(test-mock-sensors PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-mock-sensors stm32f429zi_stm32f4discovery)
+
+#add_executable(test-deployment
+#    src/tests/catch/fsm/test-deployment.cpp
+#    ${TEST_UTILS_SOURCES}
+#    ${DEPLOYMENT_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-deployment PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-deployment PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-deployment stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-deployment-interactive
+#    src/tests/deployment/test-deployment-interactive.cpp
+#    ${TEST_UTILS_SOURCES}
+#    ${DEPLOYMENT_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-deployment-interactive PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-deployment-interactive PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-deployment-interactive stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-airbrakes
+#    src/tests/catch/fsm/test-airbrakes.cpp
+#    ${TEST_UTILS_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#    ${AIRBRAKES_SOURCES}
+#)
+#target_include_directories(test-airbrakes PRIVATE ${OBSW_INCLUDE_DIRS})
+#target_compile_definitions(test-airbrakes PRIVATE STANDALONE_CATCH1_TEST)
+#sbs_target(test-airbrakes stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-airbrakes-interactive
+    src/tests/airbrakes/test-airbrakes-interactive.cpp
+    ${TEST_UTILS_SOURCES}
+    ${LOGSERVICE_SOURCES}
+    ${AIRBRAKES_SOURCES}
+)
+target_include_directories(test-airbrakes-interactive PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(test-airbrakes-interactive stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-airbrakes-algorithm
+#    src/tests/airbrakes/test-airbrakes-algorithm.cpp
+#    ${LOGSERVICE_SOURCES}
+#    ${AIRBRAKES_SOURCES}
+#)
+#target_include_directories(test-airbrakes-algorithm PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-airbrakes-algorithm stm32f429zi_skyward_death_stack_x)
+
+#-----------------------------------------------------------------------------#
+#         Tests - Comprehensive (included in death-stack-x-testsuite)         #
+#-----------------------------------------------------------------------------#
+
+#add_executable(test-power-board
+#    src/tests/deathstack-boards/test-power-board.cpp
+#    ${TEST_UTILS_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#    ${AIRBRAKES_SOURCES}
+#)
+#target_include_directories(test-power-board PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-power-board stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-stm-board src/tests/deathstack-boards/test-stm-board.cpp)
+#target_include_directories(test-stm-board PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-stm-board stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-rf-board src/tests/deathstack-boards/test-rf-board.cpp)
+#target_include_directories(test-rf-board PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-rf-board stm32f429zi_skyward_death_stack_x)
+
+#add_executable(test-analog-board
+#    src/tests/deathstack-boards/test-analog-board.cpp
+#    ${PINHANDLER_SOURCES}
+#    ${LOGSERVICE_SOURCES}
+#)
+#target_include_directories(test-analog-board PRIVATE ${OBSW_INCLUDE_DIRS})
+#sbs_target(test-analog-board stm32f429zi_skyward_death_stack_x)
diff --git a/Makefile b/Makefile
deleted file mode 100644
index 342f705f5ac319c7bb50447bfc7a6516e68d8fc3..0000000000000000000000000000000000000000
--- a/Makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-all:
-	@echo "MAKEFILE WRAPPER. PLEASE USE ./sbs FOR A BETTER EXPERIENCE :)"
-	@python3 skyward-boardcore/sbs -v
-
-.PHONY: clean
-
-clean:
-	@echo "MAKEFILE WRAPPER. PLEASE USE ./sbs FOR A BETTER EXPERIENCE :)"
-	@python3 skyward-boardcore/sbs -c
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..fe291af39618a49ba67b9411397551970313eff4
--- /dev/null
+++ b/cmake/dependencies.cmake
@@ -0,0 +1,124 @@
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Author: Damiano Amatruda
+#
+# 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.
+
+set(OBSW_INCLUDE_DIRS src src/boards/DeathStack)
+
+set(DEATHSTACK_NEW_SOURCES
+    src/boards/DeathStack/events/EventStrings.cpp
+    src/boards/DeathStack/PinHandler/PinHandler.cpp
+    src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp
+    src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
+    src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
+    src/boards/DeathStack/Main/Radio.cpp
+    src/boards/DeathStack/Main/Sensors.cpp
+    src/boards/DeathStack/Main/StateMachines.cpp
+    src/boards/DeathStack/Deployment/DeploymentController.cpp
+    src/boards/DeathStack/FlightModeManager/FMMController.cpp
+    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
+    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
+    src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp
+    src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
+    src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
+    src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
+)
+#set(DEATHSTACK_SOURCES
+#    src/boards/DeathStack/LoggerService/LoggerService.cpp
+#    src/boards/DeathStack/events/EventStrings.cpp
+#    src/boards/DeathStack/FlightModeManager/FMMController.cpp
+#    src/boards/DeathStack/SensorManager/SensorManager.cpp
+#    src/boards/DeathStack/Deployment/DeploymentController.cpp
+#    src/boards/DeathStack/PinHandler/PinHandler.cpp
+#    src/boards/DeathStack/TMTCManager/TMTCManager.cpp
+#    src/boards/DeathStack/LoggerService/TmRepository.cpp
+#    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
+#    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
+#    src/boards/DeathStack/LoggerService/FSRController.cpp
+#    src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp
+#    src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
+#)
+set(ADA_SOURCES
+    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
+    src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
+)
+set(DEPLOYMENT_SOURCES
+    src/boards/DeathStack/Deployment/DeploymentController.cpp
+)
+set(AIRBRAKES_SOURCES
+    src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
+)
+set(PINHANDLER_SOURCES
+    src/boards/DeathStack/PinHandler/PinHandler.cpp
+)
+set(FMM_SOURCES
+    src/boards/DeathStack/FlightModeManager/FMMController.cpp
+)
+#set(SENSORS_SOURCES
+#    src/boards/DeathStack/Sensors/BMX160Calibrator.cpp
+#    skyward-boardcore/src/shared/sensors/calibration/SensorDataExtra.cpp
+#)
+#set(TMTC_SOURCES
+#    src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
+#    src/boards/DeathStack/TelemetriesTelecommands/TMTCManager.cpp
+#)
+set(ALDEERAN_SOURCES
+    src/boards/Ignition/IgnitionManager.cpp
+)
+set(LOGSERVICE_SOURCES
+    src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
+    src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
+)
+set(ADA_TEST_SOURCES
+    src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp
+)
+set(KALMAN_TEST_SOURCES
+    src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp
+)
+set(MOCK_SENSORS_DATA_SOURCES
+    src/mocksensors/lynx_flight_data/lynx_imu_data.cpp
+    src/mocksensors/lynx_flight_data/lynx_press_data.cpp
+    src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp
+    src/mocksensors/lynx_flight_data/lynx_gps_data.cpp
+    src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp
+)
+set(HERMES_TESTS_SOURCES
+    src/tests/catch/fsm/test-ada.cpp
+)
+set(RAM_TEST_SOURCES
+    src/tests/ram_test/sha1.cpp
+)
+set(EVT_FUNCTIONS_SOURCES
+    src/boards/DeathStack/events/EventStrings.cpp
+)
+set(NAVIGATION_SYSTEM_SOURCES
+    src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp
+    src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
+)
+set(TESTS_OBSW_SOURCES
+    src/tests/catch/fsm/test-fmm.cpp
+    src/tests/catch/fsm/test-tmtc.cpp
+    src/tests/catch/fsm/test-ada.cpp
+    src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+    src/tests/catch/fsm/test-deployment.cpp
+    src/tests/catch/fsm/test-flightstatsrecorder.cpp
+    src/tests/catch/fsm/test-airbrakes.cpp
+    #src/tests/catch/fsm/test-nas.cpp
+    #src/tests/catch/nas/test-nas-simulation.cpp
+)
diff --git a/ide/vscode/c_cpp_properties.json b/ide/vscode/c_cpp_properties.json
index d86fc75194e21468497092214c3f4877d69d0193..3e76f3ac827cc564842aced1ebd0ea599eeb683e 100755
--- a/ide/vscode/c_cpp_properties.json
+++ b/ide/vscode/c_cpp_properties.json
@@ -1,62 +1,63 @@
 {
-  "configurations": [
-    {
-      "name": "stm32f429zi_skyward_death_stack_x",
-      "cStandard": "c11",
-      "cppStandard": "c++11",
-      "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
-      "defines": [
-        "DEBUG",
-        "_ARCH_CORTEXM4_STM32F4",
-        "_BOARD_stm32f429zi_skyward_death_stack",
-        "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack",
-        "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",
-        "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
-        "${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/simple-template-matrix",
-        "${workspaceFolder}/skyward-boardcore/libs/fmt/include",
-        "${workspaceFolder}/skyward-boardcore/libs/eigen",
-        "${workspaceFolder}/skyward-boardcore/libs",
-        "${workspaceFolder}/skyward-boardcore/src/shared",
-        "${workspaceFolder}/skyward-boardcore/src/tests",
-        "${workspaceFolder}/src/boards/DeathStack",
-        "${workspaceFolder}/src/boards"
-      ],
-      "browse": {
-        "path": [
-          "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
-          "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
-          "${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/simple-template-matrix",
-          "${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/boards",
-          "${workspaceFolder}/src/tests"
-        ],
-        "limitSymbolsToIncludedHeaders": true
-      }
-    }
-  ],
-  "version": 4
-}
+    "configurations": [
+        {
+            "name": "stm32f429zi_skyward_death_stack_x",
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_stm32f429zi_skyward_death_stack",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack",
+                "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",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                "${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/simple-template-matrix",
+                "${workspaceFolder}/skyward-boardcore/libs/fmt/include",
+                "${workspaceFolder}/skyward-boardcore/libs/eigen",
+                "${workspaceFolder}/skyward-boardcore/libs",
+                "${workspaceFolder}/skyward-boardcore/src/shared",
+                "${workspaceFolder}/skyward-boardcore/src/tests",
+                "${workspaceFolder}/src/boards/DeathStack",
+                "${workspaceFolder}/src/boards"
+            ],
+            "browse": {
+                "path": [
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+                    "${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/simple-template-matrix",
+                    "${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/boards",
+                    "${workspaceFolder}/src/tests"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "configurationProvider": "ms-vscode.cmake-tools"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file
diff --git a/sbs b/sbs
index bd5c8097bccc3c27ccc9dc2323f32358d5436871..a1ff845e48246000f8ac98621c86767dd6c2b65b 100755
--- a/sbs
+++ b/sbs
@@ -1,3 +1,3 @@
-#!/bin/bash
+#!/bin/sh
 
-python3 skyward-boardcore/sbs "$@"
+"$(dirname "$0")/skyward-boardcore/sbs" "$@"
diff --git a/sbs.bat b/sbs.bat
index f06689800efe1943e1dfa54f13ab90891758c2f3..c79aeda7e03382590465b97a391000a79326df05 100644
--- a/sbs.bat
+++ b/sbs.bat
@@ -1 +1 @@
-python -u skyward-boardcore/sbs -n %*
\ No newline at end of file
+skyward-boardcore/sbs -n %*
diff --git a/sbs.conf b/sbs.conf
deleted file mode 100644
index 2231ba0fe3b13630312e356078bb2b82c894d739..0000000000000000000000000000000000000000
--- a/sbs.conf
+++ /dev/null
@@ -1,681 +0,0 @@
-# Skyward Build System Configuration File
-#
-# Copyright (c) 2015-2019 Skyward Experimental Rocketry
-# Authors: Alain Carlucci, Alvise de' Faveri Tron
-#
-# 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.
-#
-# Here's a list of available BoardIds:
-#lpc2138_miosix_board
-#stm32f103ze_stm3210e-eval
-#stm32f103ve_mp3v2
-#stm32f100rb_stm32vldiscovery
-#stm32f103ve_strive_mini
-#stm32f103ze_redbull_v2
-#stm32f407vg_stm32f4discovery
-#stm32f207ig_stm3220g-eval
-#stm32f207zg_ethboard_v2
-#stm32f207ze_als_camboard
-#stm32l151_als_mainboard
-#stm32f407vg_bitsboard
-#stm32f205rg_sony-newman
-#stm32f429zi_stm32f4discovery
-#stm32f103cb_als_mainboard_rev2
-#stm32f100cb_tempsensor
-#stm32f429zi_oledboard2
-#efm32gg332f1024_wandstem
-#stm32f411re_nucleo
-#stm32f429zi_skyward_anakin
-#stm32f100rc_solertegiard
-#stm32f205rc_skyward_stormtrooper
-#stm32f401vc_stm32f4discovery
-#stm32f103c8_breakout
-#stm32f100c8_microboard
-#stm32f469ni_stm32f469i-disco
-#stm32f429zi_skyward_death_stack
-#stm32f429zi_skyward_death_stack_x
-#stm32f401re_nucleo
-#stm32f103c8_skyward_aldeeran
-
-
-# Syntax of this file:
-# SBS_PROJECT: project settings
-# srcfiles:    groups of sources to be compiled together
-# boards:      main files, located in ENTRY_PATH
-# tests:       test files, located in TESTS_PATH
-
-#-----------------------------------#
-#            SBS Project            #
-#-----------------------------------#
-# ENTRY_PATH:       entrypoints folder realative path
-# TESTS_PATH:       tests folder realative path
-# SRC_PATH:         shared sources folder realative path
-# SBS_BASE:         boardcore folder relative path
-# PROJECT_INCLUDES: Optional additional include directories, preceeded by -I (eg: -Ilibs/mxgui)
-# PROJECT_SUBDIRS:  Optional subfolders which contains additional makefiles (eg: libs/mxgui)
-# PROJECT_LIBS:     Optional static libraries. (eg: libs/mxgui/mxgui.a)
-
-[SBS_PROJECT]
-Type:             project
-ENTRY_PATH:       src/entrypoints
-TESTS_PATH:       src/tests
-SRC_PATH:         src
-SBS_BASE:         skyward-boardcore
-PROJECT_INCLUDES: -Isrc
-                  -Isrc/boards/DeathStack
-                  -Iskyward-boardcore/src/shared
-                  -Iskyward-boardcore/libs/mavlink_skyward_lib
-                  -Iskyward-boardcore/libs/eigen
-                  -Iskyward-boardcore/libs/fmt
-PROJECT_SUBDIRS:
-PROJECT_LIBS:
-
-#-----------------------------------#
-#          List of Sources          #
-#-----------------------------------#
-# [name]    -> can be included by boards and tests using '%name'
-#     Type: srcfiles
-#     Files: a '\n'-separated list of f
-
-[deathstack-new]
-Type:       srcfiles
-Files:      src/boards/DeathStack/events/EventStrings.cpp
-            src/boards/DeathStack/PinHandler/PinHandler.cpp
-            src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp
-            src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
-            src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
-            src/boards/DeathStack/Main/Radio.cpp
-            src/boards/DeathStack/Main/Sensors.cpp
-            src/boards/DeathStack/Main/StateMachines.cpp
-            src/boards/DeathStack/Deployment/DeploymentController.cpp
-            src/boards/DeathStack/FlightModeManager/FMMController.cpp
-            src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
-            src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
-            src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp
-            src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
-            src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
-            src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
-            
-[deathstack]
-Type:       srcfiles
-Files:      src/boards/DeathStack/LoggerService/LoggerService.cpp
-            src/boards/DeathStack/events/EventStrings.cpp
-            src/boards/DeathStack/FlightModeManager/FMMController.cpp
-            src/boards/DeathStack/SensorManager/SensorManager.cpp
-            src/boards/DeathStack/Deployment/DeploymentController.cpp
-            src/boards/DeathStack/PinHandler/PinHandler.cpp
-            src/boards/DeathStack/TMTCManager/TMTCManager.cpp
-            src/boards/DeathStack/LoggerService/TmRepository.cpp
-            src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
-            src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
-            src/boards/DeathStack/LoggerService/FSRController.cpp
-            src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp
-            src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
-
-[ada]
-Type:       srcfiles
-Files:      src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp
-            src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp
-
-[deployment]
-Type:       srcfiles
-Files:      src/boards/DeathStack/Deployment/DeploymentController.cpp
-
-[airbrakes]
-Type:       srcfiles
-Files:      src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp
-
-[pinhandler]
-Type:       srcfiles
-Files:      src/boards/DeathStack/PinHandler/PinHandler.cpp
-
-[fmm]
-Type:       srcfiles
-Files:      src/boards/DeathStack/FlightModeManager/FMMController.cpp
-
-[sensors]
-Type:       srcfiles
-Files:      src/boards/DeathStack/Sensors/BMX160Calibrator.cpp
-            skyward-boardcore/src/shared/sensors/calibration/SensorDataExtra.cpp
-
-[tmtc]
-Type:       srcfiles
-Files:      src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp
-            src/boards/DeathStack/TelemetriesTelecommands/TMTCManager.cpp
-
-[aldeeran]
-Type:       srcfiles
-Files:      src/boards/Ignition/IgnitionManager.cpp
-
-[logservice]
-Type:       srcfiles
-Files:      src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp
-            src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp
-
-[ada-test-sources]
-Type:       srcfiles
-Files:      src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp
-
-[kalman-test-sources]
-Type:       srcfiles
-Files:      src/tests/catch/ada/kalman_acc/test-kalman-acc-data.cpp
-
-[mock-sensors-data]
-Type:       srcfiles
-Files:      src/mocksensors/lynx_flight_data/lynx_imu_data.cpp
-            src/mocksensors/lynx_flight_data/lynx_press_data.cpp
-            src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp
-            src/mocksensors/lynx_flight_data/lynx_gps_data.cpp
-            src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp
-
-[hermes-tests]
-Type:       srcfiles
-Files:      src/tests/catch/fsm/test-ada.cpp
-
-[ram-test]
-Type:       srcfiles
-Files:      src/tests/ram_test/sha1.cpp
-
-[evt-functions]
-Type:       srcfiles
-Files:      src/boards/DeathStack/events/EventStrings.cpp
-
-[navigation-system]
-Type:       srcfiles
-Files:      src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.cpp
-            src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp
-
-[tests-obsw]
-Type:       srcfiles
-Files:      src/tests/catch/fsm/test-fmm.cpp
-            src/tests/catch/fsm/test-tmtc.cpp
-            src/tests/catch/fsm/test-ada.cpp
-            src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
-            src/tests/catch/fsm/test-deployment.cpp
-            src/tests/catch/fsm/test-flightstatsrecorder.cpp
-            src/tests/catch/fsm/test-airbrakes.cpp
-            #src/tests/catch/fsm/test-nas.cpp
-            #src/tests/catch/nas/test-nas-simulation.cpp
-           
-#--------------------------#
-#          Boards          #
-#--------------------------#
-# [boardname]  -> compile using './sbs -b boardname'
-#     BoardId: see the list at the top of this file
-#     BinName: name of the final binary (without extension!)
-#     Include: a space-separated list of files, %something will be substituted
-#              with the corresponding 'srcfiles'
-#     Main:    name of the main file (e.g. 'foo' -> src/entrypoints/foo.cpp)
-
-[death-stack-x-entry]
-Type:       board
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    death-stack-x-entry
-Libs:       fmt eigen
-Include:    %shared %deathstack-new %math %ubloxgps %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration %mock-sensors-data
-Defines:    -DFLIGHT -DEUROC -DNDEBUG
-Main:       death-stack-x-entry
-
-[death-stack-x-testsuite]
-Type:       board
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    death-stack-x-testsuite
-Libs:       fmt eigen
-Include:    %shared %deathstack-new %internal-adc %pwm %hbridge %test-utils %servo %spi %ubloxgps %ads1118 %ms5803 %bmx160 %math %xbee %bmx160withcorrection %sensormanager %calibration
-Defines:    -DDEBUG
-Main:       death-stack-x-testsuite
-
-# [death-stack-entry]
-# Type:       board
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    death-stack-entry
-# Include:    %shared %deathstack %pwm %hbridge %spi %xbee %piksi %servo
-# Defines:    -DDEATH_STACK_1
-# Main:       death-stack-entry
-
-# [death-stack-testsuite]
-# Type:       board
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    death-stack-testsuite
-# Include:    %shared %deathstack %pwm %xbee %piksi %servo
-# Defines:    -DDEATH_STACK_1
-# Main:       death-stack-testsuite
-
-# [windtunnel-entry]
-# Type:       board
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    windtunnel-entry
-# Include:    %shared %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118
-# Defines:    
-# Main:       windtunnel-entry
-
-# [hil-entry]
-# Type:       board
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    hil-entry
-# Include:    %shared %sensormanager %fmm %pinhandler %pwm %servo %ada %navigation-system %math %logservice  %deployment %hbridge
-# Defines:    -DHARDWARE_IN_THE_LOOP -DDEBUG
-# Main:       hardware_in_the_loop/hil-entry
-
-#--------------------------#
-#          Tests           #
-#--------------------------#
-# [testname]   -> compile using './sbs -b testname'
-#     BoardId: see the list at the top of this file
-#     BinName: name of the final binary (without extension!)
-#     Include: a space-separated list of files, %something will be substituted
-#              with the corresponding 'srcfiles'
-#     Main:    name of the main file (e.g. 'foo' -> src/tests/foo.cpp)
-
-# [catch-tests-entry]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    catch-tests-entry
-# Include:    %shared %deathstack %pwm %xbee %piksi %servo %hermes-tests %ada-test-sources %mock-sensors-data
-# Defines:    -DDEATH_STACK_1
-# Main:       catch/catch-tests-entry
-
-[tests-catch]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    tests-catch
-Include:    %shared %deathstack-new %tests-obsw %ada-test-sources %mock-sensors-data %math %ubloxgps %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration
-Defines:    
-Libs:       fmt eigen
-Main:       catch/catch-tests-entry
-
-## Driver tests
-
-# [test-imus]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-imus
-# Include:    %shared %sensors 
-# Defines:    -DDEATH_STACK_1
-# Main:       drivers/test-imus
-
-# [test-cutter]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-cutter
-# Include:    %pwm %hbridge %internal-adc %shared
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       drivers/test-cutter
-
-# [test-mavlink]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-mavlink
-# Include:    %shared %spi %tmtc  %xbee %logservice
-# Defines:    -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1
-# Main:       drivers/test-mavlink
-
-# [ledwave]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    ledwave
-# Include:    %shared
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       ledwave
-
-[ramtest]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    ramtest
-Include:    %ram-test
-Defines:    -D__ENABLE_XRAM
-Main:       ram_test/ramtest
-
-# [test-motor]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-motor
-# Include:    %shared %deployment %pwm %servo
-# Defines:
-# Main:       drivers/test-motor
-
-# [test-servo]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-servo
-# Include:    %shared %servo %pwm %logservice %airbrakes
-# Defines:
-# Libs:       fmt
-# Main:       drivers/test-servo
-
-# [test-hse]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-hse
-# Include:    
-# Defines:
-# Main:       test-hse
-
-# [test-all-sensors]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-all-sensors
-# Include:    %shared %piksi
-# Defines:    -DDEATH_STACK_1
-# Main:       drivers/test-all-sensors
-
-# [test-pressure-calib]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-pressure-calib
-# Include:    %shared %piksi
-# Defines:    -DDEATH_STACK_1
-# Main:       drivers/test-pressure-calib
-
-## Component tests
-
-# [test-canproxy]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-canproxy
-# Include:    %shared %canproxy  %canbus
-# Defines:    -DDEBUG
-# Main:       test-canproxy
-
-[test-pinhandler]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    test-pinhandler
-Include:    %shared %pinhandler %logservice
-Defines:    -DDEBUG
-Libs:       fmt
-Main:       test-pinhandler
-
-# [test-logproxy]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-logproxy
-# Include:    %shared  %logservice
-# Defines:    -DDEBUG -DDEATH_STACK_1
-# Main:       test-logproxy
-
-# [test-sensormanager]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-sensormanager
-# Include:    %shared %sensors  %piksi %logservice %ada-test-sources
-# Defines:    -DDEBUG -USE_MOCK_SENSORS -DDEATH_STACK_1
-# Main:       test-sensormanager
-
-#Cant use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class
-# [test-sm+tmtc]
-# Type:       test
-# BoardId:    st %adam32f429zi_skyward_death_stack
-# BinName:    test-sm+tmtc
-# Include:    %shared %sensors %logservice  %ada %piksi %tmtc %xbee %evt-functions %ada-test-sources
-# Defines:    -DDEBUG -DDEATH_STACK_1
-# Main:       test-sm+tmtc
-
-# [test-fmm]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-fmm
-# Include:    %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration
-# Defines:    -DSTANDALONE_CATCH1_TEST
-# Libs:       fmt eigen
-# Main:       catch/fsm/test-fmm
-
-# [test-fmm-interactive]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-fmm-interactive
-# Include:    %shared %deathstack-new %math %servo %xbee %pwm %spi %hbridge %ads1118 %internal-adc %sensormanager %ubloxgps %bmx160 %bmx160withcorrection %calibration
-# Defines:    -DDEBUG
-# Libs:       fmt eigen
-# Main:       test-fmm-interactive
-
-## FSM tests
-
-# [test-ada]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-ada
-# Include:    %shared %ada %test-utils %ada-test-sources %mock-sensors-data %logservice
-# Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG
-# Libs:       fmt
-# Main:       catch/fsm/test-ada
-
-# [test-ada-simulation]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-ada-simulation
-# Include:    %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %bmx160 %bmx160withcorrection %internal-adc %calibration
-# Defines:    -DDEBUG -DSTANDALONE_CATCH1_TEST
-# Libs:       fmt eigen
-# Main:       catch/ada/ada_kalman_p/test-ada-simulation
-
-[test-ada-dpl-simulation]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    test-ada-dpl-simulation
-Include:    %shared %ada-test-sources %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration
-Defines:    -DDEBUG
-Libs:       fmt eigen
-Main:       test-ada-dpl-simulation
-
-[test-nas]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    test-nas
-Include:    %shared %navigation-system %test-utils %mock-sensors-data %math %logservice
-Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG
-Libs:       fmt
-Main:       catch/fsm/test-nas
-
-[test-nas-simulation]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    test-nas-simulation
-Include:    %shared %navigation-system %mock-sensors-data %math %sensormanager %logservice
-Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG
-Libs:       fmt
-Main:       catch/nas/test-nas-simulation
-
-# [test-tmtc]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-tmtc
-# Include:    %shared %test-utils %math %ubloxgps %deathstack-new %pwm %hbridge %spi %xbee %servo %sensormanager %ads1118 %ms5803 %bmx160 %bmx160withcorrection %internal-adc %calibration
-# Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG -DEIGEN_MAX_ALIGN_BYTES=0
-# Libs:       fmt
-# Main:       catch/fsm/test-tmtc
-
-# [test-flightstatsrecorder]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-flightstatsrecorder
-# Include:    %shared %test-utils %logservice
-# Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG
-# Libs:       fmt
-# Main:       catch/fsm/test-flightstatsrecorder
-
-# [test-ignition]
-# Type: testada_controller
-# BoardId: stm32f429zi_skyward_death_stack_x
-# BinName: test-ignition
-# Include:  %shared %ignition  %canbus %canproxy %test-utils
-# Defines: -DSTANDALONE_CATCH1_TEST
-# Main: catch/fsm/test-ignition
-
-# HIL testing algorithms
-
-# [test-SerialInterface]
-# Type:       test
-# BoardId:    stm32f407vg_stm32f4discovery
-# BinName:    test-SerialInterface
-# Include:    %shared
-# Defines:    -DHIL_SERIALINTERFACE -DDEBUG
-# Libs:       fmt
-# Main:       hardware_in_the_loop/test-SerialInterface/test-SerialInterface
-
-# [test-HIL]
-# Type:       test
-# BoardId:    stm32f407vg_stm32f4discovery
-# BinName:    test-HIL
-# Include:    %shared %sensormanager %servo %pwm
-# Defines:    -DHIL -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL/test-HIL
-
-# [test-HIL+Aerobrake]
-# Type:       test
-# BoardId:    stm32f407vg_stm32f4discovery
-# BinName:    test-HIL+Aerobrake
-# Include:    %shared %sensormanager 
-# Defines:    -DHIL_AEROBRAKE -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake
-
-# [test-HIL+AerobrakeController]
-# Type:       test
-# BoardId:    stm32f407vg_stm32f4discovery
-# BinName:    test-HIL+AerobrakeController
-# Include:    %shared %sensormanager 
-# Defines:    -DHIL_AEROBRAKE -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL+AerobrakeController/test-HIL+AerobrakeController
-
-# [test-HIL+ADA]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-HIL+ADA
-# Include:    %shared %sensormanager %ada %ada-test-sources %logservice 
-# Defines:    -DHIL_ADA -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL+ApogeeDetectionAlgorithm/test-HIL+ADA
-
-# [test-HIL+ADA+Aerobrake]
-# Type:       test
-# BoardId:    stm32f407vg_stm32f4discovery
-# BinName:    test-HIL+ADA+Aerobrake
-# Include:    %shared %sensormanager %ada %ada-test-sources %logservice 
-# Defines:    -DHIL_ADA_AEROBRAKE -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake
-
-# [test-HIL+ADA+AerobrakeController+nas]
-# Type:       test
-# BoardId:    stm32f429zi_stm32f4discovery
-# BinName:    test-HIL+ADA+AerobrakeController+nas
-# Include:    %shared %sensormanager %ada %ada-test-sources %logservice  %navigation-system %math
-# Defines:    -DHIL_ADA_AEROBRAKECONTROLLER_NAS -DDEBUG
-# Main:       hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas
-
-# [eigen-test]
-# Type:       test
-# BoardId:    stm32f429zi_stm32f4discovery
-# BinName:    eigen-test
-# Include:    %shared
-# Defines:    -DDEBUG
-# Main:       eigen-test
-
-# [test-mock-sensors]
-# Type:       test
-# BoardId:    stm32f429zi_stm32f4discovery
-# BinName:    test-mock-sensors
-# Include:    %shared %mock-sensors-data
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       mock_sensors/test-mock-sensors
-
-# [test-deployment]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-deployment
-# Include:    %shared %test-utils %pwm %servo %deployment %hbridge %logservice
-# Defines:    -DSTANDALONE_CATCH1_TEST
-# Libs:       fmt
-# Main:       catch/fsm/test-deployment
-
-# [test-deployment-interactive]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-deployment-interactive
-# Include:    %shared %test-utils %pwm %servo %deployment %hbridge %logservice
-# Defines:    -DSTANDALONE_CATCH1_TEST -DDEBUG
-# Libs:       fmt
-# Main:       deployment/test-deployment-interactive
-
-# [test-airbrakes]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-airbrakes
-# Include:    %shared %test-utils %pwm %servo %logservice %airbrakes
-# Defines:    -DSTANDALONE_CATCH1_TEST
-# Libs:       fmt
-# Main:       catch/fsm/test-airbrakes
-
-[test-airbrakes-interactive]
-Type:       test
-BoardId:    stm32f429zi_skyward_death_stack_x
-BinName:    test-airbrakes-interactive
-Include:    %shared %test-utils %pwm %servo %logservice %airbrakes
-Defines:    -DDEBUG
-Libs:       fmt
-Main:       airbrakes/test-airbrakes-interactive
-
-# [test-airbrakes-algorithm]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-airbrakes-algorithm
-# Include:    %shared %servo %pwm %logservice %airbrakes
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       airbrakes/test-airbrakes-algorithm
-
-# Comprehensive DeathStackX tests
-# (included in death-stack-x-testsuite)
-
-# [test-power-board]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-power-board
-# Include:    %shared %internal-adc %pwm %hbridge %test-utils %servo %logservice %airbrakes
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       deathstack-boards/test-power-board
-
-# [test-stm-board]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-stm-board
-# Include:    %shared
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       deathstack-boards/test-stm-board
-
-# [test-rf-board]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-rf-board
-# Include:    %shared %spi %ubloxgps %bmx160
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       deathstack-boards/test-rf-board
-
-# [test-analog-board]
-# Type:       test
-# BoardId:    stm32f429zi_skyward_death_stack_x
-# BinName:    test-analog-board
-# Include:    %shared %spi %ads1118 %pinhandler %logservice
-# Defines:    -DDEBUG
-# Libs:       fmt
-# Main:       deathstack-boards/test-analog-board
diff --git a/skyward-boardcore b/skyward-boardcore
index f7b31e8d85d4c04ad136b6fef5d7a780c2897725..b731f89cae858bda25e87516cb8c200036c474f1 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit f7b31e8d85d4c04ad136b6fef5d7a780c2897725
+Subproject commit b731f89cae858bda25e87516cb8c200036c474f1
diff --git a/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h
index c2e6360ac468a4a3ea05b5075806a30472b1b4ce..ca8be01c8a60b07dc337c5ab101a872ab0439541 100644
--- a/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h
+++ b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h
@@ -27,7 +27,7 @@
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-align"
 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
-#include <mavlink_skyward_lib/mavlink_lib/lynx/mavlink.h>
+#include <mavlink_lib/lynx/mavlink.h>
 #pragma GCC diagnostic pop
 
 #include <drivers/mavlink/MavlinkDriver.h>
diff --git a/src/boards/DeathStack/configs/TMTCConfig.h b/src/boards/DeathStack/configs/TMTCConfig.h
index b01bad0b304f2a6397b0b6384f68dfd4f475d349..1712fa66c14caef965acd9edc3ce6cf5baad2e36 100644
--- a/src/boards/DeathStack/configs/TMTCConfig.h
+++ b/src/boards/DeathStack/configs/TMTCConfig.h
@@ -24,7 +24,7 @@
 #include <drivers/Xbee/Xbee.h>
 #include <drivers/spi/SPIDriver.h>
 #include <interfaces-impl/hwmapping.h>
-#include <mavlink_skyward_lib/mavlink_lib/lynx/mavlink.h>
+#include <mavlink_lib/lynx/mavlink.h>
 
 namespace DeathStackBoard
 {
diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
index a2f7897ffd075988fb3b13fd064a829b3ba321f5..d88c8b7c166f3e92f3e9112a56f49d15a5e92e0a 100644
--- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
@@ -21,13 +21,15 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../../catch-tests-entry.cpp"
 #endif
 
 #define EIGEN_NO_MALLOC
 
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
+
+#include <Eigen/Dense>
 
 #define private public
 #define protected public
diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
index 883cd1fd46d1db641417d69b5675a505675a94c4..8169ac0fe347a5ba15477a9e3ed7b1f35024cb83 100644
--- a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
+++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
@@ -21,7 +21,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../../catch-tests-entry.cpp"
 #endif
 
 #define private public
@@ -33,7 +33,7 @@
 #include <iostream>
 #include <random>
 #include <sstream>
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "test-kalman-acc-data.h"
 
diff --git a/src/tests/catch/ada/test-rogallo-dts.cpp b/src/tests/catch/ada/test-rogallo-dts.cpp
index 86dbb8ee5391ccbac27bc98dd727a819fefcd847..ac887b85e2ec6014a3f346fab5fa35d0d61a58f6 100644
--- a/src/tests/catch/ada/test-rogallo-dts.cpp
+++ b/src/tests/catch/ada/test-rogallo-dts.cpp
@@ -21,12 +21,12 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 #include <utils/EventCounter.h>
 
 #include <algorithm>
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "ADA/RogalloDTS/ElevationMap.h"
 #include "ADA/RogalloDTS/LHCircles.h"
diff --git a/src/tests/catch/catch-tests-entry.cpp b/src/tests/catch/catch-tests-entry.cpp
index 88504833a92870610dd948d69d719055e6f0215f..389ee6f73e86392b06b52cfaedafdd9c6fa83dfe 100644
--- a/src/tests/catch/catch-tests-entry.cpp
+++ b/src/tests/catch/catch-tests-entry.cpp
@@ -57,7 +57,7 @@
 
 #include <cstring>
 #include <string>
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 #include <vector>
 
 using miosix::Thread;
diff --git a/src/tests/catch/fsm/test-ada.cpp b/src/tests/catch/fsm/test-ada.cpp
index 5a0913c94d7e8e38f261bd21eac49c3618efec72..51ee14fe9f26c640166782ec64a2f6c7cee5b84f 100644
--- a/src/tests/catch/fsm/test-ada.cpp
+++ b/src/tests/catch/fsm/test-ada.cpp
@@ -21,7 +21,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -29,7 +29,9 @@
 
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
+
+#include <Eigen/Dense>
 
 #define private public
 #define protected public
diff --git a/src/tests/catch/fsm/test-airbrakes.cpp b/src/tests/catch/fsm/test-airbrakes.cpp
index abaa3c1e090e9efd15c0cc84a49521f2ae3683c1..0eb3af90c0141fb76f4eb463b47ff29bcbc9b558 100644
--- a/src/tests/catch/fsm/test-airbrakes.cpp
+++ b/src/tests/catch/fsm/test-airbrakes.cpp
@@ -21,7 +21,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -32,7 +32,7 @@
 #include <AirBrakes/AirBrakesServo.h>
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "configs/AirBrakesConfig.h"
 #include "events/Events.h"
diff --git a/src/tests/catch/fsm/test-deployment.cpp b/src/tests/catch/fsm/test-deployment.cpp
index 890be19a0b569b252ea20182e891bea0461f55f5..68d0df735b67720af4b5abd153a9b9185e6a1225 100644
--- a/src/tests/catch/fsm/test-deployment.cpp
+++ b/src/tests/catch/fsm/test-deployment.cpp
@@ -21,7 +21,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -32,7 +32,7 @@
 #include <drivers/servo/servo.h>
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "configs/DeploymentConfig.h"
 #include "events/Events.h"
diff --git a/src/tests/catch/fsm/test-flightstatsrecorder.cpp b/src/tests/catch/fsm/test-flightstatsrecorder.cpp
index 8d3d632cd5224b59b9ff8326006f592755b828fc..45e9cee8e22f3b5c762ea089357d26e32bdfd61e 100644
--- a/src/tests/catch/fsm/test-flightstatsrecorder.cpp
+++ b/src/tests/catch/fsm/test-flightstatsrecorder.cpp
@@ -22,7 +22,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -30,7 +30,9 @@
 
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
+
+#include <Eigen/Dense>
 
 #define private public
 #define protected public
diff --git a/src/tests/catch/fsm/test-fmm.cpp b/src/tests/catch/fsm/test-fmm.cpp
index 143651cab6ee1f1b156318ca5670b444451aec46..793a6a547141279ae08294e3be3047084002c3df 100644
--- a/src/tests/catch/fsm/test-fmm.cpp
+++ b/src/tests/catch/fsm/test-fmm.cpp
@@ -29,10 +29,12 @@
 
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "events/Events.h"
 
+#include <Eigen/Dense>
+
 #define private public
 #define protected public
 
diff --git a/src/tests/catch/fsm/test-ignition.cpp b/src/tests/catch/fsm/test-ignition.cpp
index fbda2c9275f402fec7c04fa3cd1f31256b935b12..5df0ea6a94fd93097a21ad3f71cdd25bfa090c93 100644
--- a/src/tests/catch/fsm/test-ignition.cpp
+++ b/src/tests/catch/fsm/test-ignition.cpp
@@ -21,7 +21,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -36,7 +36,7 @@
 #include <events/Events.h>
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "utils/testutils/TestHelper.h"
 
diff --git a/src/tests/catch/fsm/test-nas.cpp b/src/tests/catch/fsm/test-nas.cpp
index 2496d14a4dc7c8fe5d118f3210849d61210996fc..1d1b514969d163d5acea6b42ccf4c57c8b18ca13 100644
--- a/src/tests/catch/fsm/test-nas.cpp
+++ b/src/tests/catch/fsm/test-nas.cpp
@@ -21,10 +21,10 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 // We need access to the handleEvent(...) function in state machines in order to
 // test them synchronously
diff --git a/src/tests/catch/fsm/test-tmtc.cpp b/src/tests/catch/fsm/test-tmtc.cpp
index 9cfb17ac86c2af960f0a559095170c0ee194aae9..65b7478eb5e81b6ff214d1f42cb84099efe0e983 100644
--- a/src/tests/catch/fsm/test-tmtc.cpp
+++ b/src/tests/catch/fsm/test-tmtc.cpp
@@ -22,7 +22,7 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
 // We need access to the handleEvent(...) function in state machines in order to
@@ -30,7 +30,9 @@
 
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
+
+#include <Eigen/Dense>
 
 #define private public
 #define protected public
diff --git a/src/tests/catch/nas/test-nas-simulation.cpp b/src/tests/catch/nas/test-nas-simulation.cpp
index ac2eebc0c8626244bf15a43f3ae2223cd8ee4c15..0ba5573b65824f2fb2ac9818b3ef9a5b4e7a8177 100644
--- a/src/tests/catch/nas/test-nas-simulation.cpp
+++ b/src/tests/catch/nas/test-nas-simulation.cpp
@@ -21,10 +21,10 @@
  */
 
 #ifdef STANDALONE_CATCH1_TEST
-#include "catch/catch-tests-entry.cpp"
+#include "../catch-tests-entry.cpp"
 #endif
 
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #define private public
 
diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp
index c7b21dd23993d243ce536155b6ab25e375c4bd61..276a1ccd5a934134408b815ffe1593350d017e38 100644
--- a/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp
+++ b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp
@@ -31,7 +31,7 @@
 #include <iostream>
 #include <random>
 #include <sstream>
-#include <utils/testutils/catch.hpp>
+#include <catch2/catch.hpp>
 
 #include "TimestampTimer.h"
 #include "miosix.h"