diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 24bc7db738a3e58d3eb2b651d9b00cf9dc55724f..94d0850d8867b04e774e33ee911382d074d8f2fc 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,85 +1,136 @@
 {
+    "env": {
+        "defaultDefines": [
+            "_MIOSIX",
+            "DEBUG",
+            "__cplusplus=201402L"
+        ],
+        "defaultIncludePaths": [
+            "${workspaceFolder}/skyward-boardcore/libs/Catch2/include",
+            "${workspaceFolder}/skyward-boardcore/libs/eigen",
+            "${workspaceFolder}/skyward-boardcore/libs/fmt/include",
+            "${workspaceFolder}/skyward-boardcore/libs/mavlink-skyward-lib",
+            "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix",
+            "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common",
+            "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common/CMSIS/Include",
+            "${workspaceFolder}/skyward-boardcore/libs/mxgui",
+            "${workspaceFolder}/skyward-boardcore/libs/tscpp",
+            "${workspaceFolder}/skyward-boardcore/src/shared",
+            "${workspaceFolder}/skyward-boardcore/src/tests",
+            "${workspaceFolder}/src/boards",
+            "${workspaceFolder}/src"
+        ]
+    },
     "configurations": [
+        {
+            "name": "stm32f205rc_skyward_ciuti",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f205RC_skyward_ciuti",
+                "_BOARD_STM32F205RC_SKYWARD_CIUTI",
+                "_ARCH_CORTEXM3_STM32F2",
+                "STM32F205xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_120MHz=120000000",
+                "V_DDA_VOLTAGE=3.3f"
+            ],
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti"
+            ]
+        },
         {
             "name": "stm32f407vg_stm32f4discovery",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F4DISCOVERY",
+                "{defaultDefines}",
                 "_MIOSIX_BOARDNAME=stm32f407vg_stm32f4discovery",
+                "_BOARD_STM32F4DISCOVERY",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F407xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+                "${defaultIncludePaths}",
                 "${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"
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery"
+            ]
+        },
+        {
+            "name": "stm32f429zi_hre_test_stand",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_hre_test_stand",
+                "_BOARD_STM32F429ZI_HRE_TEST_STAND",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
-                    "${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/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
-            }
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_hre_test_stand",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_hre_test_stand"
+            ]
         },
         {
-            "name": "stm32f429zi_stm32f4discovery",
+            "name": "stm32f429zi_skyward_death_stack_v3",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v3",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_STM32F4DISCOVERY",
-                "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "HILSimulation",
-                "__cplusplus=201103L"
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3"
+            ]
+        },
+        {
+            "name": "stm32f767zi_compute_unit",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
+                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
+            ],
+            "includePath": [
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
                 "${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",
@@ -98,8 +149,8 @@
             ],
             "browse": {
                 "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
                     "${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",
@@ -124,23 +175,25 @@
             }
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_x",
+            "name": "stm32f767zi_compute_unit_HIL",
             "cStandard": "c11",
             "cppStandard": "c++14",
-            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "compilerPath": "C://arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
+                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f",
+                "HILTest"
             ],
             "includePath": [
-                "${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/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
                 "${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",
@@ -159,8 +212,8 @@
             ],
             "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/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
                     "${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",
@@ -170,7 +223,6 @@
                     "${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",
@@ -183,29 +235,31 @@
                     "${workspaceFolder}/src/tests"
                 ],
                 "limitSymbolsToIncludedHeaders": true
-            },
-            "configurationProvider": "ms-vscode.cmake-tools"
+            }
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_x_maker_faire",
+            "name": "stm32f767zi_skyward_death_stack_v4",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "HILSimulation",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X_MAKER_FAIRE",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire",
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v4",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_V4",
+                "_ARCH_CORTEXM7_STM32F7",
+                "_BOARD_STM32F767ZI_SKYWARD_DEATHST_V4",
+                "_MIOSIX_BOARDNAME=stm32f767zi_skyward_death_stack_v4",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "_MIOSIX",
-                "__cplusplus=201103L"
+                "__cplusplus=201402L",
+                "HILSimulation",
+                "ROCCARASO"
             ],
             "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/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
                 "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common",
                 "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix",
                 "${workspaceFolder}/skyward-boardcore/libs/Catch2/single_include",
@@ -216,16 +270,15 @@
                 "${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"
+                "${workspaceFolder}/src",
+                "${workspaceFolder}/src/hardware_in_the_loop"
             ],
             "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/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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",
@@ -234,7 +287,6 @@
                     "${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",
@@ -242,33 +294,33 @@
                     "${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"
+                    "${workspaceFolder}/src/boards"
                 ],
                 "limitSymbolsToIncludedHeaders": true
-            },
-            "configurationProvider": "ms-vscode.cmake-tools"
+            }
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_v3",
+            "name": "stm32f767zi_skyward_death_stack_v4_HIL",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v3",
+                "_ARCH_CORTEXM7_STM32F7",
+                "_BOARD_STM32F767ZI_SKYWARD_DEATHST_V4",
+                "_MIOSIX_BOARDNAME=stm32f767zi_skyward_death_stack_v4",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "_MIOSIX",
-                "__cplusplus=201103L"
+                "__cplusplus=201103L",
+                "HILSimulation",
+                "ROCCARASO",
+                "HILMain"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
                 "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common",
                 "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix",
                 "${workspaceFolder}/skyward-boardcore/libs/Catch2/single_include",
@@ -279,16 +331,15 @@
                 "${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"
+                "${workspaceFolder}/src",
+                "${workspaceFolder}/src/hardware_in_the_loop"
             ],
             "browse": {
                 "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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",
@@ -304,33 +355,29 @@
                     "${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"
+                    "${workspaceFolder}/src/boards"
                 ],
                 "limitSymbolsToIncludedHeaders": true
             }
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_v3_hil_euroc",
+            "name": "stm32f429zi_skyward_pyxis_auxiliary",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "DEBUG",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v3",
+                "_BOARD_STM32F429ZI_SKYWARD_PYXIS_AUXILIARY",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_pyxis_auxiliary",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "_MIOSIX",
-                "__cplusplus=201103L",
-                "HILSimulation",
-                "EUROC"
+                "__cplusplus=201103L"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
                 "${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",
@@ -349,8 +396,8 @@
             ],
             "browse": {
                 "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
+                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
                     "${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",
@@ -375,313 +422,164 @@
             }
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_v3_hil_roccaraso",
+            "name": "stm32f205rc_skyward_ciuti",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "DEBUG",
+                "_ARCH_CORTEXM3_STM32F2",
+                "_BOARD_STM32F205RC_SKYWARD_CIUTI",
+                "_MIOSIX_BOARDNAME=stm32f205RC_skyward_ciuti",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
+            ],
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
+            ],
+        },
+        {
+            "name": "stm32f429zi_skyward_death_stack_x",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v3",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L",
-                "HILSimulation",
-                "ROCCARASO"
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${defaultIncludePaths}",
                 "${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_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${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/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
-            }
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x"
+            ]
         },
         {
-            "name": "stm32f767zi_compute_unit",
+            "name": "stm32f429zi_skyward_death_stack_x_maker_faire",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
-                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
-                "_ARCH_CORTEXM7_STM32F7",
-                "STM32F769xx",
-                "HSE_VALUE=25000000",
-                "SYSCLK_FREQ_216MHz=216000000",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X_MAKER_FAIRE",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
                 "__ENABLE_XRAM",
                 "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${defaultIncludePaths}",
                 "${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_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${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/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
-            }
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire"
+            ]
         },
         {
-            "name": "stm32f767zi_compute_unit_HIL",
+            "name": "stm32f429zi_skyward_groundstation",
             "cStandard": "c11",
             "cppStandard": "c++14",
-            "compilerPath": "C://arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
-                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
-                "_ARCH_CORTEXM7_STM32F7",
-                "STM32F769xx",
-                "HSE_VALUE=25000000",
-                "SYSCLK_FREQ_216MHz=216000000",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation",
+                "_BOARD_STM32F429ZI_SKYWARD_GS",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.3f",
-                "HILTest"
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
+                "${defaultIncludePaths}",
                 "${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_v3",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                    "${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/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
-            }
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation"
+            ]
         },
         {
-            "name": "stm32f767zi_skyward_death_stack_v4",
+            "name": "stm32f429zi_skyward_groundstation_parafoil",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM7_STM32F7",
-                "_BOARD_STM32F767ZI_SKYWARD_DEATHST_V4",
-                "_MIOSIX_BOARDNAME=stm32f767zi_skyward_death_stack_v4",
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_parafoil",
+                "D_BOARD_STM32F429ZI_SKYWARD_GS_PARAFOIL",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201402L",
-                "HILSimulation",
-                "ROCCARASO"
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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/boards",
-                "${workspaceFolder}/src",
-                "${workspaceFolder}/src/hardware_in_the_loop"
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil"
+            ]
+        },
+        {
+            "name": "stm32f429zi_skyward_groundstation_v2",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_v2",
+                "_BOARD_STM32F429ZI_SKYWARD_GS_V2",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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/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"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            }
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2"
+            ]
         },
         {
-            "name": "stm32f767zi_skyward_death_stack_v4_HIL",
+            "name": "stm32f429zi_skyward_parafoil",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM7_STM32F7",
-                "_BOARD_STM32F767ZI_SKYWARD_DEATHST_V4",
-                "_MIOSIX_BOARDNAME=stm32f767zi_skyward_death_stack_v4",
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_parafoil",
+                "_BOARD_STM32F429ZI_SKYWARD_PARAFOIL",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L",
-                "HILSimulation",
-                "ROCCARASO",
-                "HILMain"
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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/boards",
-                "${workspaceFolder}/src",
-                "${workspaceFolder}/src/hardware_in_the_loop"
-            ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_skyward_death_stack_v4",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/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/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"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            }
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil"
+            ]
         },
         {
             "name": "stm32f429zi_skyward_pyxis_auxiliary",
@@ -689,308 +587,182 @@
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_PYXIS_AUXILIARY",
+                "{defaultDefines}",
                 "_MIOSIX_BOARDNAME=stm32f429zi_skyward_pyxis_auxiliary",
+                "_BOARD_STM32F429ZI_SKYWARD_PYXIS_AUXILIARY",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
+                "${defaultIncludePaths}",
                 "${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_pyxis_auxiliary",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
-                    "${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/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
-            }
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary"
+            ]
         },
         {
-            "name": "stm32f205rc_skyward_ciuti",
+            "name": "stm32f429zi_skyward_rig",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM3_STM32F2",
-                "_BOARD_STM32F205RC_SKYWARD_CIUTI",
-                "_MIOSIX_BOARDNAME=stm32f205RC_skyward_ciuti",
-                "HSE_VALUE=25000000",
-                "SYSCLK_FREQ_120MHz=120000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_rig",
+                "_BOARD_STM32F429ZI_SKYWARD_RIG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/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/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/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/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
-            }
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rig",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_rig"
+            ]
         },
         {
-            "name": "stm32f429zi_skyward_parafoil",
+            "name": "stm32f429zi_stm32f4discovery",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery",
+                "_BOARD_STM32F429ZI_STM32F4DISCOVERY",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_PARAFOIL",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_parafoil",
+                "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
+                "${defaultIncludePaths}",
                 "${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/mavlink-skyward-lib",
-                "${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"
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery"
+            ]
+        },
+        {
+            "name": "stm32f767zi_compute_unit",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
+                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F767xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
-                    "${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",
-                    "${workspaceFolder}/src"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            },
-            "configurationProvider": "ms-vscode.cmake-tools"
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_compute_unit",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_compute_unit"
+            ]
         },
         {
-            "name": "stm32f429zi_skyward_groundstation",
+            "name": "stm32f767zi_gemini_motor",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_GS",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_gemini_motor",
+                "_BOARD_STM32F767ZI_GEMINI_MOTOR",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F767xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
-                "${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"
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_gemini_motor",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_gemini_motor"
+            ]
+        },
+        {
+            "name": "stm32f756zg_nucleo",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f756zg_nucleo",
+                "D_BOARD_STM32F756ZG_NUCLEO",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F756xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
-                    "${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/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
-            }
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f756zg_nucleo",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f756zg_nucleo"
+            ]
         },
         {
-            "name": "stm32f429zi_skyward_groundstation_parafoil",
+            "name": "stm32f767zi_nucleo",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "DEBUG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_GS_PARAFOIL",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_parafoil",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
-                "_MIOSIX",
-                "__cplusplus=201103L"
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_nucleo",
+                "D_BOARD_STM32F767ZI_NUCLEO",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F767xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
-                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
-                "${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"
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_nucleo",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_nucleo"
+            ]
+        },
+        {
+            "name": "stm32f769ni_discovery",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "{defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f769ni_discovery",
+                "_BOARD_STM32F769NI_DISCO",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
+                "V_DDA_VOLTAGE=3.3f"
             ],
-            "browse": {
-                "path": [
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
-                    "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
-                    "${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/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
-            }
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f769ni_discovery",
+                "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f769ni_discovery"
+            ]
         }
     ],
     "version": 4
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e25d87503b136e3c61413f26b678f48e6be839f6..14369b6886f36b9b733b8f5cce41cce13e871c30 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,3 +51,11 @@ add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPU
 target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
 target_compile_definitions(main-entry-euroc PRIVATE EUROC)
 sbs_target(main-entry-euroc stm32f767zi_skyward_death_stack_v4)
+
+add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES})
+target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(motor-entry stm32f767zi_gemini_motor)
+
+#-----------------------------------------------------------------------------#
+#                               Test entrypoints                              #
+#-----------------------------------------------------------------------------#
\ No newline at end of file
diff --git a/cmake/dependencies Lynx.cmake b/cmake/dependencies Lynx.cmake
deleted file mode 100644
index d067614da27b84f9362c2191904dd59d235f94fc..0000000000000000000000000000000000000000
--- a/cmake/dependencies Lynx.cmake	
+++ /dev/null
@@ -1,136 +0,0 @@
-# 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/common
-    src/boards
-    src/boards/DeathStack
-)
-
-set(DEATHSTACK_NEW_SOURCES
-    src/common/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(PAYLOAD_SOURCES
-    src/common/events/EventStrings.cpp
-    src/boards/Payload/WingControl/WingServo.cpp
-    src/boards/Payload/Main/Sensors.cpp
-    src/boards/Payload/Main/Radio.cpp
-    src/boards/Payload/PinHandler/PinHandler.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/cmake/dependencies Pyxis.cmake b/cmake/dependencies Pyxis.cmake
deleted file mode 100644
index d99e2d6ccae5bc186c4fd652969358a7406b0c5c..0000000000000000000000000000000000000000
--- a/cmake/dependencies Pyxis.cmake	
+++ /dev/null
@@ -1,89 +0,0 @@
-# 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/boards
-    src/hardware_in_the_loop
-    src/tests/hardware_in_the_loop
-)
-
-set(AUXILIARY_COMPUTER
-    src/boards/Auxiliary/Actuators/Actuators.cpp
-    src/boards/Auxiliary/CanHandler/CanHandler.cpp
-)
-
-set(MAIN_COMPUTER
-    src/boards/Main/Actuators/Actuators.cpp
-    src/boards/Main/CanHandler/CanHandler.cpp
-    src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
-    src/boards/Main/Sensors/Sensors.cpp
-    src/boards/Main/PinHandler/PinHandler.cpp
-    src/boards/Main/Radio/Radio.cpp
-    src/boards/Main/TMRepository/TMRepository.cpp
-    src/boards/Main/StateMachines/AirBrakesController/AirBrakesController.cpp
-    src/boards/Main/StateMachines/ADAController/ADAController.cpp
-    src/boards/Main/StateMachines/Deployment/Deployment.cpp
-    src/boards/Main/StateMachines/NASController/NASController.cpp
-    src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
-)
-
-set(HIL
-    src/hardware_in_the_loop/HIL/HILTransceiver.cpp
-    src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
-)
-
-set(PAYLOAD_COMPUTER
-    src/boards/Payload/Actuators/Actuators.cpp
-    src/boards/Payload/CanHandler/CanHandler.cpp
-    src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
-    src/boards/Payload/Sensors/Sensors.cpp
-    src/boards/Payload/PinHandler/PinHandler.cpp
-    src/boards/Payload/Radio/Radio.cpp
-    src/boards/Payload/TMRepository/TMRepository.cpp
-    src/boards/Payload/StateMachines/NASController/NASController.cpp
-    src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
-    src/boards/Payload/Wing/AltitudeTrigger.cpp
-    src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
-    src/boards/Payload/Wing/FileWingAlgorithm.cpp
-    src/boards/Payload/Wing/WingAlgorithm.cpp
-    src/boards/Payload/Wing/WingController.cpp
-    src/boards/Payload/StateMachines/Deployment/Deployment.cpp
-)
-
-set(PARAFOIL_COMPUTER
-    src/boards/Parafoil/Actuators/Actuators.cpp
-    src/boards/Parafoil/FlightModeManager/FlightModeManager.cpp
-    src/boards/Parafoil/NASController/NASController.cpp
-    src/boards/Parafoil/Radio/Radio.cpp
-    src/boards/Parafoil/Sensors/Sensors.cpp
-    src/boards/Parafoil/TMRepository/TMRepository.cpp
-    src/boards/Parafoil/Wing/WingAlgorithm.cpp
-    src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
-    src/boards/Parafoil/Wing/WingController.cpp
-)
-
-set(CIUTI_COMPUTER
-    src/boards/Ciuti/Sensors/Sensors.cpp
-    src/boards/Ciuti/Algorithm/UprightDetector.cpp
-    src/boards/Ciuti/Serial/SerialWatcher.cpp
-)
-
-set(MOCK_LOGGER skyward-boardcore/src/shared/mock-Logger.cpp)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 91c76d1bbb9a664e0b10d376c2a6447333fc80b5..098b277a6b2fc13b067e467dbf74fcd6c571a31d 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -48,3 +48,10 @@ set(MAIN_COMPUTER
     src/boards/Main/StateMachines/Deployment/Deployment.cpp
     src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
 )
+
+set(MOTOR_SOURCES
+    src/boards/Motor/Actuators/Actuators.cpp
+    src/boards/Motor/Sensors/Sensors.cpp
+    src/boards/Motor/BoardScheduler.cpp
+    src/boards/Motor/CanHandler/CanHandler.cpp
+)
diff --git a/src/boards/Motor/Actuators/Actuators.cpp b/src/boards/Motor/Actuators/Actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e7050ac4d3dd718f004002a5f482ace632ebf59
--- /dev/null
+++ b/src/boards/Motor/Actuators/Actuators.cpp
@@ -0,0 +1,204 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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 "Actuators.h"
+
+#include <Motor/Configs/ActuatorsConfig.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+
+#include "ActuatorsData.h"
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Motor::ActuatorsConfig;
+
+namespace Motor
+{
+
+Actuators::Actuators(TaskScheduler* sched) : scheduler(sched)
+{
+    servoMain =
+        new Servo(SERVO_MAIN_TIMER, SERVO_MAIN_PWM_CH, MIN_PULSE, MAX_PULSE);
+    servoVenting = new Servo(SERVO_VENTING_TIMER, SERVO_VENTING_PWM_CH,
+                             MIN_PULSE, MAX_PULSE);
+
+    // Set the default openings
+    openings[ServosList::MAIN_VALVE]    = DEFAULT_MAIN_MAXIMUM_APERTURE;
+    openings[ServosList::VENTING_VALVE] = DEFAULT_VENTING_MAXIMUM_APERTURE;
+
+    // Set the default opening times
+    openingTimes[ServosList::MAIN_VALVE]    = DEFAULT_MAIN_OPENING_TIME;
+    openingTimes[ServosList::VENTING_VALVE] = DEFAULT_VENTING_OPENING_TIME;
+
+    // Set the opening / closing events
+    openingEvents[ServosList::MAIN_VALVE] =
+        Common::Events::MOTOR_OPEN_FEED_VALVE;
+    openingEvents[ServosList::VENTING_VALVE] =
+        Common::Events::MOTOR_OPEN_VENTING_VALVE;
+
+    closingEvents[ServosList::MAIN_VALVE] =
+        Common::Events::MOTOR_CLOSE_FEED_VALVE;
+    closingEvents[ServosList::VENTING_VALVE] =
+        Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+}
+
+bool Actuators::start()
+{
+    servoMain->enable();
+    servoVenting->enable();
+
+    uint8_t result = scheduler->addTask([=]() { checkTimings(); },
+                                        SERVO_TIMINGS_CHECK_PERIOD);
+    return result != 0;
+}
+
+float Actuators::getServoPosition(ServosList servo)
+{
+    // Get the choosen servo and nullptr in case not present
+    Servo* requestedServo = getServo(servo);
+
+    if (requestedServo == nullptr)
+    {
+        return -1;
+    }
+
+    return requestedServo->getPosition();
+}
+
+void Actuators::openServoAtomic(ServosList servo, uint32_t time)
+{
+    PauseKernelLock lock;
+
+    if (getServo(servo) != nullptr)
+    {
+        // Open the valve if it's closed
+        if (timings[servo] == 0)
+        {
+            timings[servo] = getTick() + time;
+            setFlag[servo] = getTick();
+        }
+    }
+}
+
+void Actuators::closeServo(ServosList servo)
+{
+    PauseKernelLock lock;
+
+    if (getServo(servo) != nullptr)
+    {
+        // Close the valve if it's open
+        if (timings[servo] > 0)
+        {
+            timings[servo] = 0;
+            setFlag[servo] = getTick();
+        }
+    }
+}
+
+void Actuators::setServoPosition(ServosList servo, float position)
+{
+    // To lock the resources use a kernel lock
+    PauseKernelLock lock;
+
+    // Get the choosen servo and nullptr in case not present
+    Servo* requestedServo = getServo(servo);
+
+    if (!(requestedServo == nullptr))
+    {
+        requestedServo->setPosition(position);
+
+        // Log the position
+        ActuatorsData data;
+        data.timestamp = TimestampTimer::getTimestamp();
+        data.servoId   = servo;
+        data.position  = position;
+        Logger::getInstance().log(data);
+    }
+}
+
+Servo* Actuators::getServo(ServosList servo)
+{
+    switch (servo)
+    {
+        case MAIN_VALVE:
+            return servoMain;
+        case VENTING_VALVE:
+            return servoVenting;
+        default:
+            return nullptr;
+    }
+}
+
+void Actuators::checkTimings()
+{
+    uint64_t currentTick = getTick();
+
+    // Enter in protected zone where the timings should be checked and changed
+    // and the servo should be positioned atomically over all the threads. A
+    // kernel lock is adopted only due to it's performance but a mutex could be
+    // easily adopted.
+    {
+        PauseKernelLock lock;
+
+        for (uint8_t i = 0; i < ServosList::ServosList_ENUM_END; i++)
+        {
+            if (timings[i] > currentTick)
+            {
+                if (currentTick > setFlag[i] + SERVO_CONFIDENCE_TIME)
+                {
+                    // 2% less than the actual aperture
+                    setServoPosition(
+                        static_cast<ServosList>(i),
+                        openings[i] - openings[i] * SERVO_CONFIDENCE);
+                }
+                else
+                {
+                    // Open the corresponding valve wrt the maximum opening
+                    setServoPosition(static_cast<ServosList>(i), openings[i]);
+                }
+            }
+            else
+            {
+                if (timings[i] != 0)
+                {
+                    timings[i] = 0;
+                    setFlag[i] = currentTick;
+                }
+                if (currentTick > setFlag[i] + SERVO_CONFIDENCE_TIME)
+                {
+                    // 2% open
+                    setServoPosition(static_cast<ServosList>(i),
+                                     openings[i] * SERVO_CONFIDENCE);
+                }
+                else
+                {
+                    setServoPosition(static_cast<ServosList>(i), 0);
+                }
+            }
+        }
+    }
+}
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Actuators/Actuators.h b/src/boards/Motor/Actuators/Actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..61caeefedd26475d715467d7621aba1d709d8a28
--- /dev/null
+++ b/src/boards/Motor/Actuators/Actuators.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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 <actuators/Servo/Servo.h>
+#include <common/Events.h>
+#include <common/Mavlink.h>
+#include <events/EventBroker.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class Actuators : public Boardcore::Module
+{
+public:
+    /**
+     * @brief Construct a new Actuators object
+     *
+     * @param sched The scheduler to respect the atomic timings in case of TARS0
+     * engaged
+     */
+    explicit Actuators(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Enables all the servos PWMs and adds to the scheduler a periodic
+     * method to check whether the single servos time expired
+     */
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Get the specified Servo's Position
+     *
+     * @param servo The Mavlink requested servo
+     * @return float The servo position in normalized notation [0-1]
+     */
+    float getServoPosition(ServosList servo);
+
+    /**
+     * @brief Opens the servo valve passed via parameter for a certain amount of
+     * time. If the valve is already open it closes it.
+     *
+     * @param servo The servo valve to open
+     * @param time The time in [ms]
+     */
+    void openServoAtomic(ServosList servo, uint32_t time);
+
+    /**
+     * @brief Closes the servo valve passed via parameter.
+     *
+     * @param servo The servo valve to open
+     */
+    void closeServo(ServosList servo);
+
+private:
+    /**
+     * @brief Set the Servo's position to the parameter one
+     *
+     * @param servo The servo to move
+     * @param position A normalized position [0, 1]
+     */
+    void setServoPosition(ServosList servo, float position);
+
+    Boardcore::Servo* getServo(ServosList servo);
+
+    /**
+     * @brief Checks that the atomic timings for the servos didn't expire and
+     * sets the positioning according to them.
+     * @note Utilizes also the variable setFlag to understand when the position
+     * was set the last time. If greater than CONSTANT seconds ago, the function
+     * sets a little offset to the servo to avoid power angriness.
+     */
+    void checkTimings();
+
+    // Create the list of timings for every servo
+    uint64_t timings[ServosList::ServosList_ENUM_END] = {0};
+
+    // This set of flags helps the controller to know when the servo have been
+    // set, in order to change slightly their angle after CONSTANT time, to
+    // avoid over consumption
+    uint64_t setFlag[ServosList::ServosList_ENUM_END]      = {0};
+    float openings[ServosList::ServosList_ENUM_END]        = {0};
+    uint64_t openingTimes[ServosList::ServosList_ENUM_END] = {0};
+
+    // This set represents the events to throw at opening/closing of valves
+    uint8_t openingEvents[ServosList::ServosList_ENUM_END] = {0};
+    uint8_t closingEvents[ServosList::ServosList_ENUM_END] = {0};
+
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    Boardcore::Servo* servoMain;
+    Boardcore::Servo* servoVenting;
+};
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Actuators/ActuatorsData.h b/src/boards/Motor/Actuators/ActuatorsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..311c65735fea0f865012ad1d630b36d95150e3d7
--- /dev/null
+++ b/src/boards/Motor/Actuators/ActuatorsData.h
@@ -0,0 +1,58 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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 <stdint.h>
+
+#include <ostream>
+
+namespace Motor
+{
+
+struct ActuatorsData
+{
+    uint64_t timestamp;
+    uint8_t servoId;
+    float position;
+
+    ActuatorsData()
+    {
+        timestamp = 0;
+        servoId   = 0;
+        position  = 0;
+    }
+
+    ActuatorsData(uint64_t time, uint8_t servo, float pos)
+        : timestamp(time), servoId(servo), position(pos)
+    {
+    }
+
+    static std::string header() { return "timestamp,servoId,position\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)servoId << "," << position << "\n";
+    }
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/BoardScheduler.cpp b/src/boards/Motor/BoardScheduler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62f1cb9a77beb6c65c07292c74e7414f5953de43
--- /dev/null
+++ b/src/boards/Motor/BoardScheduler.cpp
@@ -0,0 +1,67 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "BoardScheduler.h"
+
+using namespace Boardcore;
+
+namespace Motor
+{
+
+BoardScheduler::BoardScheduler()
+{
+    scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
+    scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
+    scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
+    scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
+}
+
+TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
+{
+    switch (priority.get())
+    {
+        case miosix::PRIORITY_MAX:
+            return scheduler4;
+        case miosix::PRIORITY_MAX - 1:
+            return scheduler3;
+        case miosix::PRIORITY_MAX - 2:
+            return scheduler2;
+        case miosix::MAIN_PRIORITY:
+            return scheduler1;
+        default:
+            return scheduler1;
+    }
+}
+
+bool BoardScheduler::start()
+{
+    return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
+           scheduler4->start();
+}
+
+bool BoardScheduler::isStarted()
+{
+    return scheduler1->isRunning() && scheduler2->isRunning() &&
+           scheduler3->isRunning() && scheduler4->isRunning();
+}
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/BoardScheduler.h b/src/boards/Motor/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bb0643e9ae2fcf5dfc0c08175f358a4116e41d6
--- /dev/null
+++ b/src/boards/Motor/BoardScheduler.h
@@ -0,0 +1,65 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+/**
+ * @brief Class that wraps the 4 main task schedulers of the entire OBSW.
+ * There is a task scheduler for every miosix priority
+ */
+class BoardScheduler : public Boardcore::Module
+{
+public:
+    BoardScheduler();
+
+    /**
+     * @brief Get the Scheduler object relative to the requested priority
+     *
+     * @param priority The task scheduler priority
+     * @return Boardcore::TaskScheduler& Reference to the requested task
+     * scheduler.
+     * @note Min priority scheduler is returned in case of non valid priority.
+     */
+    Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Returns if all the schedulers are up and running
+     */
+    bool isStarted();
+
+private:
+    Boardcore::TaskScheduler* scheduler1;
+    Boardcore::TaskScheduler* scheduler2;
+    Boardcore::TaskScheduler* scheduler3;
+    Boardcore::TaskScheduler* scheduler4;
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Buses.h b/src/boards/Motor/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..899fde6bbc0cf07e07fff4fc0588414ab5aef839
--- /dev/null
+++ b/src/boards/Motor/Buses.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 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 <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <miosix.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class Buses : public Boardcore::Module
+{
+public:
+    Boardcore::SPIBus spi1;
+    Boardcore::SPIBus spi3;
+    Boardcore::SPIBus spi4;
+
+    Boardcore::USART usart2;
+
+    Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), usart2(USART2, 115200) {}
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/CanHandler/CanHandler.cpp b/src/boards/Motor/CanHandler/CanHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ef05206248bcfc0996d8ac70e146d2d01a5a05a
--- /dev/null
+++ b/src/boards/Motor/CanHandler/CanHandler.cpp
@@ -0,0 +1,264 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "CanHandler.h"
+
+#include <Motor/Actuators/Actuators.h>
+#include <Motor/Configs/CanHandlerConfig.h>
+#include <Motor/Sensors/Sensors.h>
+#include <common/CanConfig.h>
+#include <common/Events.h>
+
+#include <functional>
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace Common;
+using namespace CanConfig;
+using namespace Motor::CanHandlerConfig;
+
+namespace Motor
+{
+
+CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+{
+    // Configure the CAN driver
+    CanbusDriver::AutoBitTiming bitTiming;
+    bitTiming.baudRate    = BAUD_RATE;
+    bitTiming.samplePoint = SAMPLE_POINT;
+    CanbusDriver::CanbusConfig config;
+
+    driver = new CanbusDriver(CAN2, config, bitTiming);
+
+    // Create the protocol with the defined driver
+    protocol =
+        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
+                        miosix::PRIORITY_MAX - 1);
+
+    // Accept messages only from the main and RIG board
+    protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::RIG),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    driver->init();
+}
+
+bool CanHandler::start()
+{
+    // 0 if fail
+    auto result1 = scheduler->addTask(
+        [&]()
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+
+            protocol->enqueueData(static_cast<uint8_t>(Priority::HIGH),
+                                  static_cast<uint8_t>(PrimaryType::SENSORS),
+                                  static_cast<uint8_t>(Board::MOTOR),
+                                  static_cast<uint8_t>(Board::BROADCAST),
+                                  static_cast<uint8_t>(SensorId::CC_PRESSURE),
+                                  static_cast<PressureData>(
+                                      sensors->getChamberPressureSensorData()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE),
+                static_cast<PressureData>(
+                    sensors->getTankPressureSensor1Data()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE),
+                static_cast<PressureData>(
+                    sensors->getTankPressureSensor2Data()));
+        },
+        PRESSURES_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    auto result2 = scheduler->addTask(
+        [&]()
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::TANK_TEMPERATURE),
+                static_cast<TemperatureData>(sensors->getMAX31856Data()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT),
+                static_cast<CurrentData>(sensors->getServoCurrentData()));
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::SENSORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE),
+                static_cast<BatteryVoltageSensorData>(
+                    sensors->getBatteryData()));
+        },
+        TEMPERATURE_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    auto result3 = scheduler->addTask(
+        [&]()
+        {
+            protocol->enqueueSimplePacket(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::STATUS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST), initStatus, initStatus);
+        },
+        STATUS_TRANSMISSION_PERIOD);
+
+    auto result4 = scheduler->addTask(
+        [&]()
+        {
+            ModuleManager &modules = ModuleManager::getInstance();
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::ACTUATORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(ServosList::MAIN_VALVE),
+                ServoData{TimestampTimer::getTimestamp(), 0, 0,
+                          modules.get<Actuators>()->getServoPosition(
+                              ServosList::MAIN_VALVE)});
+
+            protocol->enqueueData(
+                static_cast<uint8_t>(Priority::HIGH),
+                static_cast<uint8_t>(PrimaryType::ACTUATORS),
+                static_cast<uint8_t>(Board::MOTOR),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(ServosList::VENTING_VALVE),
+                ServoData{TimestampTimer::getTimestamp(), 0, 0,
+                          modules.get<Actuators>()->getServoPosition(
+                              ServosList::VENTING_VALVE)});
+        },
+        ACTUATORS_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER);
+
+    return protocol->start() && result1 != 0 && result2 != 0 && result3 != 0 &&
+           result4 != 0;
+}
+
+bool CanHandler::isStarted()
+{
+    return protocol->isStarted() && scheduler->isRunning();
+}
+
+void CanHandler::sendEvent(EventId event)
+{
+    protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+                           static_cast<uint8_t>(PrimaryType::EVENTS),
+                           static_cast<uint8_t>(Board::MOTOR),
+                           static_cast<uint8_t>(Board::BROADCAST),
+                           static_cast<uint8_t>(event));
+}
+
+void CanHandler::setInitStatus(bool initResult) { initStatus = initResult; }
+
+void CanHandler::handleCanMessage(const CanMessage &msg)
+{
+    PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
+
+    // Depending on the received message, call the handling method
+    switch (msgType)
+    {
+        case PrimaryType::EVENTS:
+        {
+            handleCanEvent(msg);
+            break;
+        }
+        case PrimaryType::COMMAND:
+        {
+            handleCanCommand(msg);
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported message type: type={}",
+                     msgType);
+            break;
+        }
+    }
+}
+
+void CanHandler::handleCanEvent(const CanMessage &msg)
+{
+    EventId eventId = static_cast<EventId>(msg.getSecondaryType());
+
+    switch (eventId)
+    {
+        case EventId::CALIBRATE:
+        {
+            auto sensors = ModuleManager::getInstance().get<Sensors>();
+            sensors->calibrate();
+
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported event: id={}", eventId);
+            break;
+        }
+    }
+}
+
+void CanHandler::handleCanCommand(const CanMessage &msg)
+{
+    uint64_t payload = msg.payload[0];
+    ServosList servo = static_cast<ServosList>(msg.getSecondaryType());
+    bool targetState = static_cast<uint8_t>(payload);
+    uint32_t delay   = static_cast<uint32_t>(payload >> 8);
+
+    auto actuators = ModuleManager::getInstance().get<Actuators>();
+
+    if (actuators != nullptr)
+    {
+        if (targetState)
+        {
+            // printf("Opening %d for %ld\n", msg.getSecondaryType(), delay);
+            actuators->openServoAtomic(servo, delay);
+        }
+        else
+        {
+            // printf("Closing %d for %ld\n", msg.getSecondaryType(), delay);
+            actuators->closeServo(servo);
+        }
+    }
+}
+
+}  // namespace Motor
diff --git a/src/boards/Motor/CanHandler/CanHandler.h b/src/boards/Motor/CanHandler/CanHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ccc2b90a4c43e77250b634882491a866f71e2b5
--- /dev/null
+++ b/src/boards/Motor/CanHandler/CanHandler.h
@@ -0,0 +1,84 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, 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 <common/CanConfig.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class CanHandler : public Boardcore::Module
+{
+
+public:
+    explicit CanHandler(Boardcore::TaskScheduler *sched);
+
+    /**
+     * @brief Adds the periodic task to the scheduler and starts the protocol
+     * threads
+     */
+    bool start();
+
+    /**
+     * @brief Returns true if the protocol threads are started and the scheduler
+     * is running
+     */
+    bool isStarted();
+
+    /**
+     * @brief Sends a CAN event on the bus
+     */
+    void sendEvent(Common::CanConfig::EventId event);
+
+    /**
+     * @brief Set the initialization flag to true
+     */
+    void setInitStatus(bool initResult);
+
+private:
+    /**
+     * @brief Handles a generic CAN message and dispatch the message to the
+     * correct handler
+     */
+    void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
+
+    // CAN message handlers
+    void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanCommand(const Boardcore::Canbus::CanMessage &msg);
+
+    // Init status
+    std::atomic<bool> initStatus{false};
+
+    // CAN interfaces
+    Boardcore::Canbus::CanbusDriver *driver;
+    Boardcore::Canbus::CanProtocol *protocol;
+
+    Boardcore::TaskScheduler *scheduler;
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+};
+
+}  // namespace Motor
diff --git a/src/boards/Motor/Configs/ActuatorsConfig.h b/src/boards/Motor/Configs/ActuatorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..cafe8b06372dcf28bcc6a98fc1f3929f172bdb21
--- /dev/null
+++ b/src/boards/Motor/Configs/ActuatorsConfig.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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 <drivers/timer/PWM.h>
+#include <drivers/timer/TimerUtils.h>
+
+namespace Motor
+{
+
+namespace ActuatorsConfig
+{
+
+static TIM_TypeDef* const SERVO_MAIN_TIMER    = TIM8;
+static TIM_TypeDef* const SERVO_VENTING_TIMER = TIM8;
+
+constexpr Boardcore::TimerUtils::Channel SERVO_MAIN_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+constexpr Boardcore::TimerUtils::Channel SERVO_VENTING_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+
+constexpr uint16_t MIN_PULSE = 900;
+constexpr uint16_t MAX_PULSE = 2000;
+
+constexpr uint16_t SERVO_TIMINGS_CHECK_PERIOD = 100;
+constexpr uint16_t SERVO_CONFIDENCE_TIME      = 500;       // 0.5s
+constexpr float SERVO_CONFIDENCE              = 1 / 50.0;  // 2%
+
+constexpr uint32_t DEFAULT_MAIN_OPENING_TIME    = 6000;   // 6s
+constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000;  // 15s
+
+constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE    = 0.97f;
+constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE = 0.42f;
+
+}  // namespace ActuatorsConfig
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Configs/CanHandlerConfig.h b/src/boards/Motor/Configs/CanHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..4eb5340fffb564cd872ac9a4bdc7fb68766d99d3
--- /dev/null
+++ b/src/boards/Motor/Configs/CanHandlerConfig.h
@@ -0,0 +1,40 @@
+/* Copyright (c) 2023 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 <common/Events.h>
+
+namespace Motor
+{
+
+namespace CanHandlerConfig
+{
+
+constexpr unsigned int PRESSURES_TRANSMISSION_PERIOD   = 20;    // [ms]
+constexpr unsigned int ACTUATORS_TRANSMISSION_PERIOD   = 20;    // [ms]
+constexpr unsigned int TEMPERATURE_TRANSMISSION_PERIOD = 100;   // [ms]
+constexpr unsigned int STATUS_TRANSMISSION_PERIOD      = 1000;  // [ms]
+
+}  // namespace CanHandlerConfig
+
+}  // namespace Motor
diff --git a/src/boards/Motor/Configs/SensorsConfig.h b/src/boards/Motor/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..44b13b762ad92deb64f7870ab34d8a1fdac827d0
--- /dev/null
+++ b/src/boards/Motor/Configs/SensorsConfig.h
@@ -0,0 +1,199 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: 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 <Motor/Sensors/Sensors.h>
+
+namespace Motor
+{
+
+namespace SensorsConfig
+{
+
+constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE_CH =
+    Boardcore::InternalADC::Channel::CH15;
+// The battery is connected to the stm32's adc with a voltage divider
+constexpr float ADC_BATTERY_VOLTAGE_COEFF = (10 + 20) / 10;
+
+// Boardcore::LSM6DSRXConfig LSM6_SENSOR_CONFIG{
+//     .bdu       = Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE,
+//     .odrAcc    = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_1660,
+//     .opModeAcc = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
+//     .fsAcc     = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16,
+//     .odrGyr    = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_1660,
+//     .opModeGyr = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL,
+//     .fsGyr     = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000,
+//     .fifoMode  = Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS,
+//     .fifoTimestampDecimation =
+//         Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1,
+//     .fifoTemperatureBdr =
+//         Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED,
+//     .int1InterruptSelection = Boardcore::LSM6DSRXConfig::INTERRUPT::NOTHING,
+//     .int2InterruptSelection =
+//         Boardcore::LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD,
+//     .fifoWatermark = 170,
+// };
+// Boardcore::SPIBusConfig LSM6_SPI_CONFIG{
+//     Boardcore::SPI::ClockDivider::DIV_16,
+//     Boardcore::SPI::Mode::MODE_0,
+// };
+
+Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS_ODR =
+    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000;
+Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS_BDU =
+    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS_FSR =
+    Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+Boardcore::SPIBusConfig H3LIS_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+};
+
+Boardcore::LIS2MDL::Config LIS2_SENSOR_CONFIG{
+    .odr                = Boardcore::LIS2MDL::ODR_100_HZ,
+    .deviceMode         = Boardcore::LIS2MDL::MD_CONTINUOUS,
+    .temperatureDivider = 100,
+};
+Boardcore::SPIBusConfig LIS2_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+    Boardcore::SPI::Mode::MODE_3,
+    Boardcore::SPI::Order::MSB_FIRST,
+    Boardcore::SPI::Order::LSB_FIRST,
+};
+
+Boardcore::LPS22DF::Config LPS22_SENSOR_CONFIG{
+    .odr = Boardcore::LPS22DF::ODR_100,
+    .avg = Boardcore::LPS22DF::AVG_512,
+};
+Boardcore::SPIBusConfig LPS22_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_16,
+    Boardcore::SPI::Mode::MODE_3,
+    Boardcore::SPI::Order::MSB_FIRST,
+    Boardcore::SPI::Order::LSB_FIRST,
+};
+
+Boardcore::ADS131M08::Config ADS131_SENSOR_CONFIG{
+    .channelsConfig =
+        {
+            {.enabled = false},
+            // For the servo motor's current sensor the current range is from 0A
+            // to 20A and the output voltage from 0.65V to 2.65V. The sensor is
+            // connected to the adc through a voltage divider with a gain
+            // of 4.166666 So at the ADC side we have maximum 0.636 and we have
+            // to set a gain of 1 on the ADC
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_1,
+            },
+            {.enabled = false},
+            {.enabled = false},
+            {.enabled = false},
+            // The analog pressure sensors are measured with a 30ohm shunt
+            // resistor. The current ranges from 4mAh to 20mAh. So we can have a
+            // voltage up to 0.6V We set a gain of 2 to maximize resolution
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+            {
+                .enabled = true,
+                .pga     = Boardcore::ADS131M08Defs::PGA::PGA_2,
+            },
+        },
+    .oversamplingRatio = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_4096,
+    .globalChopModeEnabled = true,
+};
+Boardcore::SPIBusConfig ADS131_SPI_CONFIG{
+    Boardcore::SPI::ClockDivider::DIV_8,
+    Boardcore::SPI::Mode::MODE_1,
+};
+Boardcore::ADS131M08Defs::Channel ADS131_CHAMBER_PRESSURE_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_5;
+Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_1_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_6;
+Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_2_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_7;
+Boardcore::ADS131M08Defs::Channel ADS131_SERVO_CURRENT_CH =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
+
+// The chamber pressure sensor is a 0-40bar sensor with a 4-20mA output
+// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
+// filter with 491Hz of bandwidth and unitary gain.
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float CHAMBER_PRESSURE_MAX      = 40;
+constexpr float CHAMBER_PRESSURE_MIN      = 0;
+constexpr float CHAMBER_PRESSURE_SHUNT    = 30;     // [ohm]
+constexpr float CHAMBER_PRESSURE_CURR_MIN = 0.004;  // [A]
+constexpr float CHAMBER_PRESSURE_CURR_MAX = 0.020;  // [A]
+constexpr float CHAMBER_PRESSURE_COEFF =
+    CHAMBER_PRESSURE_MAX /
+    (CHAMBER_PRESSURE_CURR_MAX - CHAMBER_PRESSURE_CURR_MIN);  // [bar/A]
+
+// The tank pressure sensors are 0-100bar sensors with a 4-20mA output
+// On the motor motherboard there a 30ohm shunt. The shunt then goes through a
+// filter with 491Hz of bandwidth and unitary gain.
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float TANK_PRESSURE_1_MAX      = 100;
+constexpr float TANK_PRESSURE_1_MIN      = 0;
+constexpr float TANK_PRESSURE_1_SHUNT    = 30;     // [ohm]
+constexpr float TANK_PRESSURE_1_CURR_MIN = 0.004;  // [A]
+constexpr float TANK_PRESSURE_1_CURR_MAX = 0.020;  // [A]
+constexpr float TANK_PRESSURE_1_COEFF =
+    TANK_PRESSURE_1_MAX /
+    (TANK_PRESSURE_1_CURR_MAX - TANK_PRESSURE_1_CURR_MIN);  // [bar/A]
+constexpr float TANK_PRESSURE_2_MAX      = 100;
+constexpr float TANK_PRESSURE_2_MIN      = 0;
+constexpr float TANK_PRESSURE_2_SHUNT    = 30;     // [ohm]
+constexpr float TANK_PRESSURE_2_CURR_MIN = 0.004;  // [A]
+constexpr float TANK_PRESSURE_2_CURR_MAX = 0.020;  // [A]
+constexpr float TANK_PRESSURE_2_COEFF =
+    TANK_PRESSURE_2_MAX /
+    (TANK_PRESSURE_2_CURR_MAX - TANK_PRESSURE_2_CURR_MIN);  // [bar/A]
+
+// The current sensor used to measure the servo motors current consumption:
+// - A range of 0A to 20A
+// - Has an output from 0.65V to 2.65V
+// - Is connected to a voltage divider with a coefficient of 12 / (38.3 + 12)
+//
+// We use only a gain coefficient because the offset is removed by the ADC
+// offset calibration feature.
+constexpr float SERVO_CURRENT_COEFF =
+    10 / (2.65 - 0.65) / (12 / (38.3 + 12));  // [A/V]
+
+// Sampling periods
+constexpr uint32_t SAMPLE_PERIOD_ADC    = 1000;
+constexpr uint32_t SAMPLE_PERIOD_LSM6   = 10;
+constexpr uint32_t SAMPLE_PERIOD_H3LIS  = 100;
+constexpr uint32_t SAMPLE_PERIOD_LIS2   = 10;
+constexpr uint32_t SAMPLE_PERIOD_LPS22  = 10;
+constexpr uint32_t SAMPLE_PERIOD_ADS131 = 1;
+constexpr uint32_t SAMPLE_PERIOD_MAX    = 10;
+
+}  // namespace SensorsConfig
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h b/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..02088ef6177206bd67cb52825cbaaac46e5ce2e2
--- /dev/null
+++ b/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2023 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 <sensors/analog/pressure/AnalogPressureSensor.h>
+
+#include "ChamberPressureSensorData.h"
+
+namespace Boardcore
+{
+
+class ChamberPressureSensor : public Sensor<ChamberPressureSensorData>
+{
+public:
+    ChamberPressureSensor(std::function<ADCData()> getVoltage,
+                          std::function<float(float)> voltageToPressure)
+        : getVoltage(getVoltage), voltageToPressure(voltageToPressure)
+    {
+        lastSample.pressure = 0;
+    }
+
+    bool init() override { return true; };
+
+    bool selfTest() override { return true; };
+
+    ///< Converts the voltage value to pressure
+    ChamberPressureSensorData sampleImpl() override
+    {
+        ADCData adc_data = getVoltage();
+
+        ChamberPressureSensorData new_data;
+        new_data.pressure          = voltageToPressure(adc_data.voltage);
+        new_data.pressureTimestamp = adc_data.voltageTimestamp;
+
+        return new_data;
+    };
+
+private:
+    ///< Function that returns the adc voltage
+    std::function<ADCData()> getVoltage;
+
+    ///< Function that converts adc voltage to current
+    std::function<float(float)> voltageToPressure;
+};
+
+}  // namespace Boardcore
diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h b/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2a5c5b3044bf1368780faadb78d09d6bb74d388
--- /dev/null
+++ b/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h
@@ -0,0 +1,40 @@
+/* Copyright (c) 2023 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 <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct ChamberPressureSensorData : public PressureData
+{
+    static std::string header() { return "timestamp,pressure\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/boards/Motor/Sensors/Sensors.cpp b/src/boards/Motor/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0643a6865d1b0d96f9d7ced1d301689efc21f478
--- /dev/null
+++ b/src/boards/Motor/Sensors/Sensors.cpp
@@ -0,0 +1,427 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Sensors.h"
+
+#include <Motor/Buses.h>
+#include <Motor/Configs/SensorsConfig.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <interfaces-impl/hwmapping.h>
+#include <logger/Logger.h>
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+using namespace Motor::SensorsConfig;
+
+namespace Motor
+{
+
+InternalADCData Sensors::getADCData()
+{
+    miosix::PauseKernelLock lock;
+    return adc != nullptr ? adc->getLastSample() : InternalADCData{};
+}
+
+BatteryVoltageSensorData Sensors::getBatteryData()
+{
+    miosix::PauseKernelLock lock;
+    return battery != nullptr ? battery->getLastSample()
+                              : BatteryVoltageSensorData{};
+}
+
+// LSM6DSRXData Sensors::getLSM6DSRXData()
+// {
+//     miosix::PauseKernelLock lock;
+//     return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+// }
+
+H3LIS331DLData Sensors::getH3LIS331DLData()
+{
+    miosix::PauseKernelLock lock;
+    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
+                                 : H3LIS331DLData{};
+}
+
+LIS2MDLData Sensors::getLIS2MDLData()
+{
+    miosix::PauseKernelLock lock;
+    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
+}
+
+LPS22DFData Sensors::getLPS22DFData()
+{
+    miosix::PauseKernelLock lock;
+    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
+}
+
+ADS131M08Data Sensors::getADS131M08Data()
+{
+    miosix::PauseKernelLock lock;
+    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+MAX31856Data Sensors::getMAX31856Data()
+{
+    miosix::PauseKernelLock lock;
+    return max31856 != nullptr ? max31856->getLastSample() : MAX31856Data{};
+}
+
+ChamberPressureSensorData Sensors::getChamberPressureSensorData()
+{
+    miosix::PauseKernelLock lock;
+    return chamberPressure != nullptr ? chamberPressure->getLastSample()
+                                      : ChamberPressureSensorData{};
+}
+
+TankPressureSensor1Data Sensors::getTankPressureSensor1Data()
+{
+    miosix::PauseKernelLock lock;
+    return tankPressure1 != nullptr ? tankPressure1->getLastSample()
+                                    : TankPressureSensor1Data{};
+}
+
+TankPressureSensor2Data Sensors::getTankPressureSensor2Data()
+{
+    miosix::PauseKernelLock lock;
+    return tankPressure2 != nullptr ? tankPressure2->getLastSample()
+                                    : TankPressureSensor2Data{};
+}
+
+CurrentData Sensors::getServoCurrentData()
+{
+    miosix::PauseKernelLock lock;
+    return servosCurrent != nullptr ? servosCurrent->getLastSample()
+                                    : CurrentData{};
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+
+Sensors::~Sensors() {}
+
+bool Sensors::start()
+{
+    adcInit();
+    batteryInit();
+    // lsm6dsrxInit();
+    h3lis331dlInit();
+    // lis2mdlInit();
+    lps22dfInit();
+    max31856Init();
+    ads131m08Init();
+    chamberPressureInit();
+    tankPressure1Init();
+    tankPressure2Init();
+    servosCurrentInit();
+
+    sensorManager = new SensorManager(sensorsMap, scheduler);
+
+    return sensorManager->start();
+}
+
+void Sensors::calibrate()
+{
+    if (ads131m08 != nullptr)
+    {
+        ads131m08->calibrateOffset(ADS131_SERVO_CURRENT_CH);
+    }
+}
+
+void Sensors::adcInit()
+{
+    adc = new InternalADC(ADC1);
+
+    adc->enableTemperature();
+    adc->enableVbat();
+    adc->enableChannel(ADC_BATTERY_VOLTAGE_CH);
+
+    SensorInfo info("ADC", SAMPLE_PERIOD_ADC,
+                    bind(&Sensors::adcCallback, this));
+
+    sensorsMap.emplace(make_pair(adc, info));
+}
+
+void Sensors::batteryInit()
+{
+    function<ADCData()> getADCVoltage(
+        bind(&InternalADC::getVoltage, adc, ADC_BATTERY_VOLTAGE_CH));
+
+    battery =
+        new BatteryVoltageSensor(getADCVoltage, ADC_BATTERY_VOLTAGE_COEFF);
+
+    SensorInfo info("BATTERY", SAMPLE_PERIOD_ADC,
+                    bind(&Sensors::batteryCallback, this));
+
+    sensorsMap.emplace(make_pair(battery, info));
+}
+
+// void Sensors::lsm6dsrxInit()
+// {
+//     SPIBus& spi1 = ModuleManager::getInstance().get<Buses>()->spi1;
+
+//     lsm6dsrx = new LSM6DSRX(spi1, peripherals::lsm6dsrx::cs::getPin(),
+//                             LSM6_SPI_CONFIG, LSM6_SENSOR_CONFIG);
+
+//     SensorInfo info("LSM6DSRX", SAMPLE_PERIOD_LSM6,
+//                     bind(&Sensors::lsm6dsrxCallback, this));
+
+//     sensorsMap.emplace(make_pair(lsm6dsrx, info));
+// }
+
+void Sensors::h3lis331dlInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    h3lis331dl = new H3LIS331DL(spi3, peripherals::h3lis331dl::cs::getPin(),
+                                H3LIS_ODR, H3LIS_BDU, H3LIS_FSR);
+
+    SensorInfo info("H3LIS331DL", SAMPLE_PERIOD_H3LIS,
+                    bind(&Sensors::h3lis331dlCallback, this));
+
+    sensorsMap.emplace(make_pair(h3lis331dl, info));
+}
+
+void Sensors::lis2mdlInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    lis2mdl = new LIS2MDL(spi3, peripherals::lis2mdl::cs::getPin(),
+                          LIS2_SPI_CONFIG, LIS2_SENSOR_CONFIG);
+
+    SensorInfo info("LIS2MDL", SAMPLE_PERIOD_LIS2,
+                    bind(&Sensors::lis2mdlCallback, this));
+
+    sensorsMap.emplace(make_pair(lis2mdl, info));
+}
+
+void Sensors::lps22dfInit()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    lps22df = new LPS22DF(spi3, peripherals::lps22df::cs::getPin(),
+                          LPS22_SPI_CONFIG, LPS22_SENSOR_CONFIG);
+
+    SensorInfo info("LPS22", SAMPLE_PERIOD_LPS22,
+                    bind(&Sensors::lps22dfCallback, this));
+
+    sensorsMap.emplace(make_pair(lps22df, info));
+}
+
+void Sensors::max31856Init()
+{
+    SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3;
+
+    max31856 = new MAX31856(spi3, peripherals::max31856::cs::getPin());
+
+    SensorInfo info("MAX31856", SAMPLE_PERIOD_MAX,
+                    bind(&Sensors::max31856Callback, this));
+
+    sensorsMap.emplace(make_pair(max31856, info));
+}
+
+void Sensors::ads131m08Init()
+{
+    SPIBus& spi4 = ModuleManager::getInstance().get<Buses>()->spi4;
+
+    ads131m08 = new ADS131M08(spi4, peripherals::ads131m08::cs::getPin(),
+                              ADS131_SPI_CONFIG, ADS131_SENSOR_CONFIG);
+
+    SensorInfo info("ADS131M08", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::ads131m08Callback, this));
+
+    sensorsMap.emplace(make_pair(ads131m08, info));
+}
+
+void Sensors::chamberPressureInit()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_CHAMBER_PRESSURE_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / CHAMBER_PRESSURE_SHUNT - CHAMBER_PRESSURE_CURR_MIN;
+            return current * CHAMBER_PRESSURE_COEFF;
+        });
+
+    chamberPressure =
+        new ChamberPressureSensor(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("CHAMBER_PRESSURE", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::chamberPressureCallback, this));
+
+    sensorsMap.emplace(make_pair(chamberPressure, info));
+}
+
+void Sensors::tankPressure1Init()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_TANK_PRESSURE_1_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / TANK_PRESSURE_1_SHUNT - TANK_PRESSURE_1_CURR_MIN;
+            return current * TANK_PRESSURE_1_COEFF;
+        });
+
+    tankPressure1 = new TankPressureSensor1(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("TANK_PRESSURE_1", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::tankPressure1Callback, this));
+
+    sensorsMap.emplace(make_pair(tankPressure1, info));
+}
+
+void Sensors::tankPressure2Init()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_TANK_PRESSURE_2_CH);
+        });
+
+    function<float(float)> voltageToPressure(
+        [](float voltage)
+        {
+            float current =
+                voltage / TANK_PRESSURE_2_SHUNT - TANK_PRESSURE_2_CURR_MIN;
+            return current * TANK_PRESSURE_2_COEFF;
+        });
+
+    tankPressure2 = new TankPressureSensor2(getADCVoltage, voltageToPressure);
+
+    SensorInfo info("TANK_PRESSURE_2", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::tankPressure2Callback, this));
+
+    sensorsMap.emplace(make_pair(tankPressure2, info));
+}
+
+void Sensors::servosCurrentInit()
+{
+    if (ads131m08 == nullptr)
+    {
+        return;
+    }
+
+    function<ADCData()> getADCVoltage(
+        [&](void)
+        {
+            auto data = ads131m08->getLastSample();
+            return data.getVoltage(ADS131_SERVO_CURRENT_CH);
+        });
+
+    function<float(float)> voltageToCurrent(
+        [](float voltage) { return voltage * SERVO_CURRENT_COEFF; });
+
+    servosCurrent = new CurrentSensor(getADCVoltage, voltageToCurrent);
+
+    SensorInfo info("SERVOS_CURRENT", SAMPLE_PERIOD_ADS131,
+                    bind(&Sensors::servosCurrentCallback, this));
+
+    sensorsMap.emplace(make_pair(servosCurrent, info));
+}
+
+void Sensors::adcCallback() { Logger::getInstance().log(adc->getLastSample()); }
+
+void Sensors::batteryCallback()
+{
+    Logger::getInstance().log(battery->getLastSample());
+}
+
+// void Sensors::lsm6dsrxCallback()
+// {
+//     Logger::getInstance().log(lsm6dsrx->getLastSample());
+// }
+
+void Sensors::h3lis331dlCallback()
+{
+    Logger::getInstance().log(h3lis331dl->getLastSample());
+}
+
+void Sensors::lis2mdlCallback()
+{
+    Logger::getInstance().log(lis2mdl->getLastSample());
+}
+
+void Sensors::lps22dfCallback()
+{
+    Logger::getInstance().log(lps22df->getLastSample());
+}
+
+void Sensors::max31856Callback()
+{
+    Logger::getInstance().log(max31856->getLastSample());
+}
+
+void Sensors::ads131m08Callback()
+{
+    Logger::getInstance().log(ads131m08->getLastSample());
+}
+
+void Sensors::chamberPressureCallback()
+{
+    Logger::getInstance().log(chamberPressure->getLastSample());
+}
+
+void Sensors::tankPressure1Callback()
+{
+    Logger::getInstance().log(tankPressure1->getLastSample());
+}
+
+void Sensors::tankPressure2Callback()
+{
+    Logger::getInstance().log(tankPressure2->getLastSample());
+}
+
+void Sensors::servosCurrentCallback()
+{
+    Logger::getInstance().log(servosCurrent->getLastSample());
+}
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/Sensors.h b/src/boards/Motor/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..780563efec9feef081b3258fed1fc4f66477854a
--- /dev/null
+++ b/src/boards/Motor/Sensors/Sensors.h
@@ -0,0 +1,124 @@
+/* Copyright (c) 2023 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 <Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h>
+#include <Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h>
+#include <Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h>
+#include <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+// #include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/MAX31856/MAX31856.h>
+#include <sensors/SensorManager.h>
+#include <sensors/analog/BatteryVoltageSensor.h>
+#include <sensors/analog/CurrentSensor.h>
+#include <sensors/analog/pressure/AnalogPressureSensor.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Motor
+{
+
+class Sensors : public Boardcore::Module
+{
+public:
+    Boardcore::InternalADCData getADCData();
+    Boardcore::BatteryVoltageSensorData getBatteryData();
+    // Boardcore::LSM6DSRXData getLSM6DSRXData();
+    Boardcore::H3LIS331DLData getH3LIS331DLData();
+    Boardcore::LIS2MDLData getLIS2MDLData();
+    Boardcore::LPS22DFData getLPS22DFData();
+    Boardcore::ADS131M08Data getADS131M08Data();
+    Boardcore::MAX31856Data getMAX31856Data();
+    Boardcore::ChamberPressureSensorData getChamberPressureSensorData();
+    Boardcore::TankPressureSensor1Data getTankPressureSensor1Data();
+    Boardcore::TankPressureSensor2Data getTankPressureSensor2Data();
+    Boardcore::CurrentData getServoCurrentData();
+
+    explicit Sensors(Boardcore::TaskScheduler* sched);
+
+    ~Sensors();
+
+    bool start();
+
+    void calibrate();
+
+private:
+    void adcInit();
+    void adcCallback();
+
+    void batteryInit();
+    void batteryCallback();
+
+    // void lsm6dsrxInit();
+    // void lsm6dsrxCallback();
+
+    void h3lis331dlInit();
+    void h3lis331dlCallback();
+
+    void lis2mdlInit();
+    void lis2mdlCallback();
+
+    void lps22dfInit();
+    void lps22dfCallback();
+
+    void max31856Init();
+    void max31856Callback();
+
+    void ads131m08Init();
+    void ads131m08Callback();
+
+    void chamberPressureInit();
+    void chamberPressureCallback();
+
+    void tankPressure1Init();
+    void tankPressure1Callback();
+
+    void tankPressure2Init();
+    void tankPressure2Callback();
+
+    void servosCurrentInit();
+    void servosCurrentCallback();
+
+    Boardcore::InternalADC* adc              = nullptr;
+    Boardcore::BatteryVoltageSensor* battery = nullptr;
+    // Boardcore::LSM6DSRX* lsm6dsrx                     = nullptr;
+    Boardcore::H3LIS331DL* h3lis331dl                 = nullptr;
+    Boardcore::LIS2MDL* lis2mdl                       = nullptr;
+    Boardcore::LPS22DF* lps22df                       = nullptr;
+    Boardcore::MAX31856* max31856                     = nullptr;
+    Boardcore::ADS131M08* ads131m08                   = nullptr;
+    Boardcore::ChamberPressureSensor* chamberPressure = nullptr;
+    Boardcore::TankPressureSensor1* tankPressure1     = nullptr;
+    Boardcore::TankPressureSensor2* tankPressure2     = nullptr;
+    Boardcore::CurrentSensor* servosCurrent           = nullptr;
+
+    Boardcore::SensorManager::SensorMap_t sensorsMap;
+    Boardcore::SensorManager* sensorManager = nullptr;
+    Boardcore::TaskScheduler* scheduler     = nullptr;
+};
+
+}  // namespace Motor
\ No newline at end of file
diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h b/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d50b299f7beef6c628726a70dc0c2b675abe017
--- /dev/null
+++ b/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2023 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 <sensors/analog/pressure/AnalogPressureSensor.h>
+
+#include "TankPressureSensor1Data.h"
+
+namespace Boardcore
+{
+
+class TankPressureSensor1 : public Sensor<TankPressureSensor1Data>
+{
+public:
+    TankPressureSensor1(std::function<ADCData()> getVoltage,
+                        std::function<float(float)> voltageToPressure)
+        : getVoltage(getVoltage), voltageToPressure(voltageToPressure)
+    {
+        lastSample.pressure = 0;
+    }
+
+    bool init() override { return true; };
+
+    bool selfTest() override { return true; };
+
+    ///< Converts the voltage value to pressure
+    TankPressureSensor1Data sampleImpl() override
+    {
+        ADCData adc_data = getVoltage();
+
+        TankPressureSensor1Data new_data;
+        new_data.pressure          = voltageToPressure(adc_data.voltage);
+        new_data.pressureTimestamp = adc_data.voltageTimestamp;
+
+        return new_data;
+    };
+
+private:
+    ///< Function that returns the adc voltage
+    std::function<ADCData()> getVoltage;
+
+    ///< Function that converts adc voltage to current
+    std::function<float(float)> voltageToPressure;
+};
+
+}  // namespace Boardcore
diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h b/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..75e241b69b29efb0f0fd55fbccb29dcfa89831c4
--- /dev/null
+++ b/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h
@@ -0,0 +1,40 @@
+/* Copyright (c) 2023 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 <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct TankPressureSensor1Data : public PressureData
+{
+    static std::string header() { return "timestamp,pressure\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h b/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h
new file mode 100644
index 0000000000000000000000000000000000000000..afd94eb93402e201dcc08013b04d0575a876d6ec
--- /dev/null
+++ b/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2023 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 <sensors/analog/pressure/AnalogPressureSensor.h>
+
+#include "TankPressureSensor2Data.h"
+
+namespace Boardcore
+{
+
+class TankPressureSensor2 : public Sensor<TankPressureSensor2Data>
+{
+public:
+    TankPressureSensor2(std::function<ADCData()> getVoltage,
+                        std::function<float(float)> voltageToPressure)
+        : getVoltage(getVoltage), voltageToPressure(voltageToPressure)
+    {
+        lastSample.pressure = 0;
+    }
+
+    bool init() override { return true; };
+
+    bool selfTest() override { return true; };
+
+    ///< Converts the voltage value to pressure
+    TankPressureSensor2Data sampleImpl() override
+    {
+        ADCData adc_data = getVoltage();
+
+        TankPressureSensor2Data new_data;
+        new_data.pressure          = voltageToPressure(adc_data.voltage);
+        new_data.pressureTimestamp = adc_data.voltageTimestamp;
+
+        return new_data;
+    };
+
+private:
+    ///< Function that returns the adc voltage
+    std::function<ADCData()> getVoltage;
+
+    ///< Function that converts adc voltage to current
+    std::function<float(float)> voltageToPressure;
+};
+
+}  // namespace Boardcore
diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h b/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..bcb18b94ded26a6afb328d7689d358fee2b24d4d
--- /dev/null
+++ b/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h
@@ -0,0 +1,40 @@
+/* Copyright (c) 2023 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 <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct TankPressureSensor2Data : public PressureData
+{
+    static std::string header() { return "timestamp,pressure\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/entrypoints/Motor/motor-entry.cpp b/src/entrypoints/Motor/motor-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d42fd5b4c35dc790a9990fd17f86ae753d08f88
--- /dev/null
+++ b/src/entrypoints/Motor/motor-entry.cpp
@@ -0,0 +1,208 @@
+/* Copyright (c) 2023 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 <Motor/Actuators/Actuators.h>
+#include <Motor/BoardScheduler.h>
+#include <Motor/Buses.h>
+#include <Motor/CanHandler/CanHandler.h>
+#include <Motor/Sensors/Sensors.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <miosix.h>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Motor;
+
+int main()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Overall status, if at some point it becomes false, there is a problem
+    // somewhere
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+
+    // Create modules
+    BoardScheduler* scheduler = new BoardScheduler();
+    Buses* buses              = new Buses();
+    auto actuators =
+        new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    auto sensors =
+        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+    auto canHandler =
+        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+
+    // Insert modules
+    if (!modules.insert<BoardScheduler>(scheduler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the board scheduler module");
+    }
+
+    if (!modules.insert<Buses>(buses))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the buses module");
+    }
+
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the sensor module");
+    }
+
+    if (!modules.insert<Sensors>(sensors))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the sensor module");
+    }
+
+    if (!modules.insert<CanHandler>(canHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the CanHandler module");
+    }
+
+    // Start modules
+    if (!Logger::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the logger module");
+    }
+
+    if (!modules.get<BoardScheduler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the board scheduler module");
+    }
+
+    if (!modules.get<Actuators>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors module");
+    }
+
+    if (!modules.get<Sensors>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors module");
+    }
+
+    if (!modules.get<CanHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the CanHandler module");
+    }
+
+    // Calibration
+    modules.get<Sensors>()->calibrate();
+
+    // Set the init status inside the CAN handler
+    modules.get<CanHandler>()->setInitStatus(initResult);
+
+    // Check the init result and launch an event
+    if (initResult)
+    {
+        miosix::led1On();
+    }
+    else
+    {
+        miosix::led2On();
+    }
+
+    while (true)
+    {
+        printf("Average CPU usage: %.1f%%\n", CpuMeter::getCpuStats().mean);
+
+        auto adcData     = sensors->getADCData();
+        auto batteryData = sensors->getBatteryData();
+        printf("[%.2fs]\tADC:\t%f %f %f\n", adcData.timestamp / 1e6,
+               adcData.temperature, adcData.vBat, batteryData.batVoltage);
+
+        // // WARNING: Fails self test
+        // auto lsm6Data = sensors->getLSM6DSRXData();
+        // printf("[%.2fs]\tLSM6:\t%fm/s^2 %fm/s^2 %fm/s^2\n",
+        //        lsm6Data.accelerationTimestamp / 1e6,
+        //        lsm6Data.accelerationX * 0.001, lsm6Data.accelerationY *
+        //        0.001, lsm6Data.accelerationZ * 0.001);
+        // printf("[%.2fs]\tLSM6:\t%frad/s %frad/s %frad/s\n",
+        //        lsm6Data.angularSpeedTimestamp / 1e6,
+        //        lsm6Data.angularSpeedX * 0.001, lsm6Data.angularSpeedY *
+        //        0.001, lsm6Data.angularSpeedZ * 0.001);
+
+        // // WARNING: The values are wrong
+        // auto h3lisData = sensors->getH3LIS331DLData();
+        // printf("[%.2fs]\tH3LIS:\t%fm/s %fm/s %fm/s\n",
+        //        h3lisData.accelerationTimestamp / 1e6,
+        //        h3lisData.accelerationX, h3lisData.accelerationY,
+        //        h3lisData.accelerationZ);
+
+        // auto lis2Data = sensors->getLIS2MDLData();
+        // printf("[%.2fs]\tLIS2:\t%f %f %f\n",
+        //        lis2Data.magneticFieldTimestamp / 1e6,
+        //        lis2Data.magneticFieldX, lis2Data.magneticFieldY,
+        //        lis2Data.magneticFieldZ);
+
+        // auto lps22Data = sensors->getLPS22DFData();
+        // printf("[%.2fs]\tLPS22:\t%f %f\n", lps22Data.pressureTimestamp / 1e6,
+        //        lps22Data.pressure, lps22Data.temperature);
+
+        // auto maxData = sensors->getMAX31856Data();
+        // printf("[%.2fs]\tMAX:\t%f° %f°\n", maxData.temperatureTimestamp /
+        // 1e6,
+        //        maxData.temperature, maxData.coldJunctionTemperature);
+
+        auto adsData = sensors->getADS131M08Data();
+        printf("[%.2fs]\tADS131:\t%f %f %f %f\n", adsData.timestamp / 1e6,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage,
+               adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage);
+
+        auto chamberData = sensors->getChamberPressureSensorData();
+        printf("[%.2fs]\tCHAMBER:\t%fbar\n",
+               chamberData.pressureTimestamp / 1e6, chamberData.pressure);
+
+        auto tank1Data = sensors->getTankPressureSensor1Data();
+        printf("[%.2fs]\tTANK1:\t\t%fbar\n", tank1Data.pressureTimestamp / 1e6,
+               tank1Data.pressure);
+
+        auto tank2Data = sensors->getTankPressureSensor2Data();
+        printf("[%.2fs]\tTANK2:\t\t%fbar\n", tank2Data.pressureTimestamp / 1e6,
+               tank2Data.pressure);
+
+        auto servoCurrent = sensors->getServoCurrentData();
+        printf("[%.2fs]\tSERVO:\t\t%fA\n", servoCurrent.currentTimestamp / 1e6,
+               servoCurrent.current);
+
+        auto batteryVoltage = sensors->getBatteryData();
+        printf("[%.2fs]\tBATTERY:\t%fV\n",
+               batteryVoltage.voltageTimestamp / 1e6,
+               batteryVoltage.batVoltage);
+
+        auto loggerData = Logger::getInstance().getStats();
+        printf("[%.2fs]\tLOGGER:\t%d\n", loggerData.timestamp / 1e6,
+               loggerData.writesFailed);
+
+        Thread::sleep(1000);
+    }
+}