diff --git a/.gitignore b/.gitignore
index 9b89790f971bdc8817c9e536f0eccafcf4009da4..a9f1775b4bc49623280a7697bbb213db7e2b4b91 100644
--- a/.gitignore
+++ b/.gitignore
@@ -32,3 +32,5 @@ core
 __pycache__
 /scripts/generators/generated
 /scripts/generators/scxmls
+
+scripts/logdecoder/logdecoder
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 36d54fe1acd93824940194e7bfda20c119aeba8f..2cbb65d50d357cbf8bf1386ac3b39b0021f77012 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -25,6 +25,7 @@ variables:
 stages:
   - build-release
   - build-debug
+  - build-logdecoder
   - test
   - lint
   - documentation
@@ -36,7 +37,7 @@ build-release:
   tags:
     - miosix
   script:
-    - ./sbs --jobs 2
+    - ./sbs
 
 # Stage build-debug
 
@@ -45,34 +46,54 @@ build-debug:
   tags:
     - miosix
   script:
-    - ./sbs --jobs 2 --debug
+    - ./sbs --debug
+
+# Stage build-logdecoder
+
+build-logdecoder:
+  stage: build-logdecoder
+  tags:
+    - miosix
+  script:
+    - cd scripts/logdecoder
+    - make
 
 # Stage test
 
 test:
   stage: test
+  tags:
+    - miosix
   script:
-    - ./sbs --jobs 2 --test catch-tests-boardcore
+    - ./sbs --test catch-tests-boardcore
 
 # Stage lint
 
 cppcheck:
   stage: lint
+  tags:
+    - miosix
   script:
     - ./scripts/linter.py --cppcheck src
 
 format:
   stage: lint
+  tags:
+    - miosix
   script:
     - ./scripts/linter.py --format src
 
 copyright:
   stage: lint
+  tags:
+    - miosix
   script:
     - ./scripts/linter.py --copyright src
 
 find:
   stage: lint
+  tags:
+    - miosix
   script:
     - ./scripts/linter.py --find src
 
diff --git a/.gitmodules b/.gitmodules
index 14ebbcc7e259961aa9036a5ccfe3a5afecb0108f..8d94e67246c0107398855117b3e192edaed08c37 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,24 +1,24 @@
-[submodule "libs/miosix-kernel"]
-	path = libs/miosix-kernel
-	url = ../miosix-kernel.git
-[submodule "libs/tscpp"]
-	path = libs/tscpp
-	url = ../third-party/tscpp.git
-[submodule "libs/mavlink_skyward_lib"]
-	path = libs/mavlink_skyward_lib
-	url = ../mavlink_gs/mavlink_skyward_lib.git
-[submodule "libs/eigen"]
+[submodule "Catch2"]
+	path = libs/Catch2
+	url = ../third-party/Catch2.git
+[submodule "Eigen"]
 	path = libs/eigen
 	url = ../third-party/eigen.git
-[submodule "libs/mxgui"]
-	path = libs/mxgui
-	url = ../third-party/mxgui.git
-[submodule "libs/fmt"]
+[submodule "Modern formatting library"]
 	path = libs/fmt
 	url = ../third-party/fmt.git
-[submodule "libs/Catch2"]
-	path = libs/Catch2
-	url = ../third-party/Catch2.git
-[submodule "libs/miosix-host"]
-	path = libs/miosix-host
-	url = ../miosix-host.git
+[submodule "MAVLink Skyward library"]
+	path = libs/mavlink-skyward-lib
+	url = ../mavlink/mavlink-skyward-lib
+[submodule "Miosix Host"]
+	path = libs/miosix-host
+	url = ../miosix-host.git
+[submodule "Miosix Kernel"]
+	path = libs/miosix-kernel
+	url = ../miosix-kernel.git
+[submodule "Miosix GUI library"]
+	path = libs/mxgui
+	url = ../third-party/mxgui.git
+[submodule "Trivial serialization for C++"]
+	path = libs/tscpp
+	url = ../third-party/tscpp.git
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index f0d67b4cd47e0a2892448b6e802ecbf24b12b721..67b3b9ff613f4759e09a39a6f2d99f273dc2df33 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -21,9 +21,10 @@
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix",
-                "${workspaceFolder}/libs/mavlink_skyward_lib",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
                 "${workspaceFolder}/libs/fmt/include",
                 "${workspaceFolder}/libs/eigen",
+                "${workspaceFolder}/libs/tscpp",
                 "${workspaceFolder}/libs",
                 "${workspaceFolder}/src/shared",
                 "${workspaceFolder}/src/tests"
@@ -41,7 +42,7 @@
                     "${workspaceFolder}/libs/miosix-kernel/miosix/util",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/*",
-                    "${workspaceFolder}/libs/mavlink_skyward_lib",
+                    "${workspaceFolder}/libs/mavlink-skyward-lib",
                     "${workspaceFolder}/libs/eigen",
                     "${workspaceFolder}/libs/tscpp",
                     "${workspaceFolder}/libs/mxgui",
@@ -73,9 +74,10 @@
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix",
-                "${workspaceFolder}/libs/mavlink_skyward_lib",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
                 "${workspaceFolder}/libs/fmt/include",
                 "${workspaceFolder}/libs/eigen",
+                "${workspaceFolder}/libs/tscpp",
                 "${workspaceFolder}/libs",
                 "${workspaceFolder}/src/shared",
                 "${workspaceFolder}/src/tests"
@@ -93,7 +95,7 @@
                     "${workspaceFolder}/libs/miosix-kernel/miosix/util",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/*",
-                    "${workspaceFolder}/libs/mavlink_skyward_lib",
+                    "${workspaceFolder}/libs/mavlink-skyward-lib",
                     "${workspaceFolder}/libs/eigen",
                     "${workspaceFolder}/libs/tscpp",
                     "${workspaceFolder}/libs/mxgui",
@@ -125,9 +127,10 @@
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
                 "${workspaceFolder}/libs/miosix-kernel/miosix",
-                "${workspaceFolder}/libs/mavlink_skyward_lib",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
                 "${workspaceFolder}/libs/fmt/include",
                 "${workspaceFolder}/libs/eigen",
+                "${workspaceFolder}/libs/tscpp",
                 "${workspaceFolder}/libs",
                 "${workspaceFolder}/src/shared",
                 "${workspaceFolder}/src/tests"
@@ -145,7 +148,60 @@
                     "${workspaceFolder}/libs/miosix-kernel/miosix/util",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
                     "${workspaceFolder}/libs/miosix-kernel/miosix/*",
-                    "${workspaceFolder}/libs/mavlink_skyward_lib",
+                    "${workspaceFolder}/libs/mavlink-skyward-lib",
+                    "${workspaceFolder}/libs/eigen",
+                    "${workspaceFolder}/libs/tscpp",
+                    "${workspaceFolder}/libs/mxgui",
+                    "${workspaceFolder}/libs/fmt",
+                    "${workspaceFolder}/src/shared",
+                    "${workspaceFolder}/src/tests"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        },
+        {
+            "name": "stm32f429zi_parafoil",
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_STM32F429ZI_PARAFOIL",
+                "_MIOSIX_BOARDNAME=stm32f429zi_parafoil",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+            "includePath": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
+                "${workspaceFolder}/libs/fmt/include",
+                "${workspaceFolder}/libs/eigen",
+                "${workspaceFolder}/libs/tscpp",
+                "${workspaceFolder}/libs",
+                "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/src/tests"
+            ],
+            "browse": {
+                "path": [
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/libs/mavlink-skyward-lib",
                     "${workspaceFolder}/libs/eigen",
                     "${workspaceFolder}/libs/tscpp",
                     "${workspaceFolder}/libs/mxgui",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 0995365f671776c5b1138c4f3c36ef31f97df10e..43e514f2603ebb9bb2b90fdab6c1c80067e2f17d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -99,32 +99,130 @@
         "ranges": "cpp",
         "compare": "cpp",
         "concepts": "cpp",
-        "qtalignedmalloc": "cpp"
+        "core": "cpp",
+        "iterativelinearsolvers": "cpp",
+        "pastixsupport": "cpp",
+        "superlusupport": "cpp",
+        "adolcforward": "cpp",
+        "alignedvector3": "cpp",
+        "bvh": "cpp",
+        "fft": "cpp",
+        "iterativesolvers": "cpp",
+        "mprealsupport": "cpp",
+        "matrixfunctions": "cpp",
+        "nonlinearoptimization": "cpp",
+        "openglsupport": "cpp",
+        "polynomials": "cpp",
+        "autodiff": "cpp",
+        "sparseextra": "cpp",
+        "specialfunctions": "cpp"
     },
     "cSpell.words": [
-        "Amatruda",
+        "aelf",
+        "Alessandro",
+        "AMSL",
+        "atthr",
         "AVDD",
-        "CPHA",
-        "Damiano",
-        "Davide",
-        "DGNSS",
-        "GNSS",
+        "Baro",
+        "boardcore",
+        "Boardcorev",
+        "Corigliano",
+        "CORTEXM",
+        "cpitch",
+        "cppcheck",
+        "croll",
+        "cwise",
+        "cyaw",
+        "deleteme",
+        "DMEIE",
+        "Doxyfile",
+        "doxygen",
+        "DRDY",
+        "Duca",
+        "Ecompass",
+        "Eigen",
+        "elfs",
+        "Erbetta",
+        "Fatt",
+        "Fatttr",
+        "fedetft's",
+        "fiprintf",
+        "Gatttr",
+        "getdetahstate",
         "Gpio",
+        "GPIOA",
         "GPIOB",
+        "GPIOC",
+        "GPIOD",
+        "GPIOE",
+        "GPIOF",
         "GPIOG",
+        "GPIOS",
+        "gpstr",
+        "Hatt",
+        "HSCMAND",
+        "HSCMRNN",
+        "IDLEIE",
+        "irqn",
+        "irqv",
+        "Kalman",
+        "Katt",
+        "kbps",
+        "ldrex",
+        "LIFCR",
+        "logdecoder",
+        "Luca",
+        "MEKF",
+        "MINC",
         "miosix",
-        "Mosi",
-        "MSTR",
-        "NMEA",
-        "nsat",
-        "PCLK",
-        "RXCRCR",
-        "RXDMAEN",
+        "mkdir",
+        "MPXHZ",
+        "Musso",
+        "NATT",
+        "NBAR",
+        "NDTR",
+        "NGPS",
+        "NMAG",
+        "NMEKF",
+        "nord",
+        "NVIC",
+        "peripehral",
+        "PINC",
+        "Pitot",
+        "Plin",
+        "Qgbw",
+        "Qget",
+        "Qhandle",
+        "Qput",
+        "Qwait",
+        "Qwakeup",
+        "Riccardo",
         "RXNE",
-        "trasfer",
-        "TXCRCR",
-        "TXDMAEN",
-        "UBXGPS"
+        "RXNEIE",
+        "sats",
+        "Satt",
+        "SDRAM",
+        "spitch",
+        "sqdip",
+        "sqtrp",
+        "sroll",
+        "SSCDANN",
+        "SSCDRRN",
+        "stof",
+        "syaw",
+        "TCIE",
+        "TEIE",
+        "testsuite",
+        "TSCPP",
+        "Ublox",
+        "Unsync",
+        "upcounter",
+        "USART",
+        "velnord",
+        "Xbee",
+        "xnord",
+        "yned"
     ],
-    "cortex-debug.variableUseNaturalFormat": false
+    "cSpell.language": "en",
+    "cSpell.enabled": true
 }
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e318826b8b8cd36d6107aca526eb9ef18c10199b..d7ac92ec3282024cfc6891855abbc3b381bcff1c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,9 @@ sbs_target(bmx160-calibration-entry stm32f429zi_skyward_death_stack_x)
 add_executable(config-dsgamma src/entrypoints/config-dsgamma.cpp)
 sbs_target(config-dsgamma stm32f429zi_stm32f4discovery)
 
+add_executable(imu-calibration src/entrypoints/imu-calibration.cpp)
+sbs_target(imu-calibration stm32f429zi_parafoil)
+
 add_executable(mxgui-helloworld src/entrypoints/examples/mxgui-helloworld.cpp)
 sbs_target(mxgui-helloworld stm32f429zi_stm32f4discovery)
 
@@ -55,9 +58,6 @@ sbs_target(sdcard-benchmark stm32f429zi_stm32f4discovery)
 #                                    Tests                                    #
 #-----------------------------------------------------------------------------#
 
-add_executable(test-kalman-eigen-benchmark src/tests/kalman/test-kalman-eigen-benchmark.cpp)
-sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery)
-
 add_executable(test-logger src/tests/logger/test-logger.cpp)
 sbs_target(test-logger stm32f429zi_stm32f4discovery)
 
@@ -80,7 +80,7 @@ add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
 sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-serial src/tests/test-serial.cpp)
-sbs_target(test-serial stm32f429zi_skyward_death_stack_x)
+sbs_target(test-serial stm32f407vg_stm32f4discovery)
 
 add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
 sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
@@ -96,7 +96,7 @@ add_executable(catch-tests-boardcore
     src/tests/catch/catch-tests-entry.cpp
     src/tests/catch/examples/example-test-factorial.cpp
     src/tests/catch/test-aero.cpp
-    src/tests/catch/test-buttonhandler.cpp
+    # src/tests/catch/test-buttonhandler.cpp
     src/tests/catch/test-circularbuffer.cpp
     src/tests/catch/test-eventbroker.cpp
     src/tests/catch/test-timestamptimer.cpp
@@ -109,6 +109,53 @@ target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS)
 sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery)
 sbs_catch_test(catch-tests-boardcore)
 
+#-----------------------------------------------------------------------------#
+#                             Tests - Actuators                               #
+#-----------------------------------------------------------------------------#
+
+add_executable(test-hbridge src/tests/actuators/test-hbridge.cpp)
+sbs_target(test-hbridge stm32f429zi_stm32f4discovery)
+
+add_executable(test-servo src/tests/actuators/test-servo.cpp)
+sbs_target(test-servo stm32f429zi_stm32f4discovery)
+
+add_executable(test-buzzer src/tests/actuators/test-buzzer.cpp)
+sbs_target(test-buzzer stm32f429zi_hre_test_stand)
+
+add_executable(test-stepper src/tests/actuators/test-stepper.cpp)
+sbs_target(test-stepper stm32f429zi_stm32f4discovery)
+
+#-----------------------------------------------------------------------------#
+#                             Tests - Algorithms                              #
+#-----------------------------------------------------------------------------#
+
+add_executable(test-kalman-eigen-benchmark src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp)
+sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery)
+
+add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp)
+sbs_target(test-tmp stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-attitude-parafoil src/tests/algorithms/NAS/test-attitude-parafoil.cpp)
+sbs_target(test-attitude-parafoil stm32f429zi_parafoil)
+
+add_executable(test-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp)
+sbs_target(test-attitude-stack stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-nas-parafoil src/tests/algorithms/NAS/test-nas-parafoil.cpp)
+sbs_target(test-nas-parafoil stm32f429zi_parafoil)
+
+add_executable(test-nas-stack src/tests/algorithms/NAS/test-nas-stack.cpp)
+sbs_target(test-nas-stack stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
+sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-triad src/tests/algorithms/NAS/test-triad.cpp)
+sbs_target(test-triad stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-triad-parafoil src/tests/algorithms/NAS/test-triad-parafoil.cpp)
+sbs_target(test-triad-parafoil stm32f429zi_parafoil)
+
 #-----------------------------------------------------------------------------#
 #                               Tests - Drivers                               #
 #-----------------------------------------------------------------------------#
@@ -128,9 +175,6 @@ sbs_target(test-dsgamma stm32f429zi_stm32f4discovery)
 add_executable(test-general-purpose-timer src/tests/drivers/timer/test-general-purpose-timer.cpp)
 sbs_target(test-general-purpose-timer stm32f429zi_stm32f4discovery)
 
-add_executable(test-hbridge src/tests/drivers/test-hbridge.cpp)
-sbs_target(test-hbridge stm32f429zi_stm32f4discovery)
-
 add_executable(test-internal-adc src/tests/drivers/test-internal-adc.cpp)
 sbs_target(test-internal-adc stm32f429zi_skyward_death_stack_x)
 
@@ -146,9 +190,6 @@ sbs_target(test-MBLoadCell stm32f407vg_stm32f4discovery)
 add_executable(test-pwm src/tests/drivers/timer/test-pwm.cpp)
 sbs_target(test-pwm stm32f429zi_stm32f4discovery)
 
-add_executable(test-servo src/tests/drivers/test-servo.cpp)
-sbs_target(test-servo stm32f429zi_stm32f4discovery)
-
 add_executable(test-spi src/tests/drivers/spi/test-spi.cpp)
 sbs_target(test-spi stm32f429zi_stm32f4discovery)
 
@@ -185,8 +226,8 @@ sbs_target(test-xbee-rcv stm32f429zi_stm32f4discovery)
 add_executable(test-xbee-snd src/tests/drivers/xbee/test-xbee-snd.cpp)
 sbs_target(test-xbee-snd stm32f429zi_stm32f4discovery)
 
-add_executable(test-stepper src/tests/drivers/stepper/test-stepper.cpp)
-sbs_target(test-stepper stm32f429zi_stm32f4discovery)
+add_executable(test-usart src/tests/drivers/usart/test-usart.cpp)
+sbs_target(test-usart stm32f407vg_stm32f4discovery)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Events                                #
@@ -233,7 +274,7 @@ add_executable(test-sx1278-serial
     src/tests/drivers/sx1278/test-sx1278-serial.cpp
     src/tests/drivers/sx1278/test-sx1278-core.cpp
 )
-sbs_target(test-sx1278-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Sensors                               #
@@ -303,7 +344,7 @@ add_executable(test-max31855 src/tests/sensors/test-max31855.cpp)
 sbs_target(test-max31855 stm32f429zi_stm32f4discovery)
 
 add_executable(test-mpu9250 src/tests/sensors/test-mpu9250.cpp)
-sbs_target(test-mpu9250 stm32f407vg_stm32f4discovery)
+sbs_target(test-mpu9250 stm32f429zi_parafoil)
 
 add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
 sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
@@ -311,6 +352,13 @@ sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
 add_executable(test-ubxgps src/tests/sensors/test-ubxgps.cpp)
 sbs_target(test-ubxgps stm32f429zi_skyward_death_stack_x)
 
+add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp)
+sbs_target(test-ubloxgps-serial stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-ubloxgps-spi src/tests/sensors/test-ubloxgps.cpp)
+target_compile_definitions(test-ubloxgps-spi PRIVATE USE_SPI)
+sbs_target(test-ubloxgps-spi stm32f407vg_stm32f4discovery)
+
 add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
 sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
 
@@ -320,3 +368,6 @@ sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
 
 add_executable(test-csvreader src/tests/utils/test-csvreader.cpp)
 sbs_target(test-csvreader stm32f429zi_stm32f4discovery)
+
+add_executable(test-buttonhandler src/tests/utils/test-buttonhandler.cpp)
+sbs_target(test-buttonhandler stm32f429zi_stm32f4discovery)
diff --git a/cmake/boardcore-host.cmake b/cmake/boardcore-host.cmake
index 712ef96983d156696b4b7e91ed999a69e0e0dacb..85d11ef2498c72e2528591aabf6a092ebb82b0af 100644
--- a/cmake/boardcore-host.cmake
+++ b/cmake/boardcore-host.cmake
@@ -25,9 +25,11 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
     ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp
     ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
 
+    # Actuators
+    ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp
+
     # Drivers
     ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp
-    ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp
 
     # Events
     ${SBS_BASE}/src/shared/events/EventBroker.cpp
@@ -35,11 +37,6 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
     # Logger
     ${SBS_BASE}/src/shared/logger/Logger.cpp
 
-    # Math
-    ${SBS_BASE}/src/shared/math/Matrix.cpp
-    ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp
-    ${SBS_BASE}/src/shared/math/Stats.cpp
-
     # Radio
     ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
 
@@ -50,11 +47,11 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
     ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
     ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
 
-    # AeroUtils
-    ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp
-
-    # TestUtils
-    ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp
+    # Utils
+    ${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp
+    ${SBS_BASE}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
+    ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp
+    ${SBS_BASE}/src/shared/utils/TestUtils/TestHelper.cpp
 )
 add_library(SkywardBoardcore::Boardcore::host ALIAS boardcore-host)
 target_include_directories(boardcore-host PUBLIC ${SBS_BASE}/src/shared)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 51a3d2ed92b9bb101bc70c39df4907ef7572653d..5c4264138caf1c3acb46abbdd7cece140b7f27ab 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -26,6 +26,14 @@ include(boardcore-host)
 foreach(OPT_BOARD ${BOARDS})
     set(BOARDCORE_LIBRARY boardcore-${OPT_BOARD})
     add_library(${BOARDCORE_LIBRARY} STATIC EXCLUDE_FROM_ALL
+        # Actuators
+        ${SBS_BASE}/src/shared/actuators/HBridge/HBridge.cpp
+        ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp
+
+
+        # Algorithms
+        ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
+
         # Debug
         ${SBS_BASE}/src/shared/utils/Debug.cpp
         ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp
@@ -35,14 +43,13 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/Canbus.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
-        ${SBS_BASE}/src/shared/drivers/hbridge/HBridge.cpp
         ${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
         ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
         ${SBS_BASE}/src/shared/drivers/timer/PWM.cpp
         ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp
         ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp
-        ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp
         ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
+        ${SBS_BASE}/src/shared/drivers/usart/USART.cpp
 
         # Events
         ${SBS_BASE}/src/shared/events/EventBroker.cpp
@@ -50,11 +57,6 @@ foreach(OPT_BOARD ${BOARDS})
         # Logger
         ${SBS_BASE}/src/shared/logger/Logger.cpp
 
-        # Math
-        ${SBS_BASE}/src/shared/math/Matrix.cpp
-        ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp
-        ${SBS_BASE}/src/shared/math/Stats.cpp
-
         # Radio
         ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp
         ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
@@ -81,14 +83,19 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
         ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
         ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
+        ${SBS_BASE}/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
         ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPS.cpp
         ${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp
 
-        # AeroUtils
-        ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp
+        # Calibration
+        ${SBS_BASE}/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
 
-        # TestUtils
-        ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp
+        # Utils
+        ${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp
+        ${SBS_BASE}/src/shared/utils/ButtonHandler/ButtonHandler.cpp
+        ${SBS_BASE}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
+        ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp
+        ${SBS_BASE}/src/shared/utils/TestUtils/TestHelper.cpp
     )
     add_library(SkywardBoardcore::Boardcore::${OPT_BOARD} ALIAS ${BOARDCORE_LIBRARY})
     target_include_directories(${BOARDCORE_LIBRARY} PUBLIC ${SBS_BASE}/src/shared)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 5e77e8cd88ceb12f063df6c60b3f9abc5d7c9e4f..d77b06c6888e6b171328204c83a08c347729fc59 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -47,4 +47,4 @@ add_subdirectory(${SBS_BASE}/libs/Catch2 EXCLUDE_FROM_ALL)
 list(APPEND CMAKE_MODULE_PATH ${SBS_BASE}/libs/Catch2/contrib)
 include(Catch)
 
-add_subdirectory(${SBS_BASE}/libs/mavlink_skyward_lib EXCLUDE_FROM_ALL)
+add_subdirectory(${SBS_BASE}/libs/mavlink-skyward-lib EXCLUDE_FROM_ALL)
diff --git a/doc/Doxyfile b/doc/Doxyfile
index 22fa2cf1651e5958dd0cc1bea9af113e7bfaf484..607537c5692282602a46f660851838436d564a88 100644
--- a/doc/Doxyfile
+++ b/doc/Doxyfile
@@ -873,7 +873,7 @@ RECURSIVE              = YES
 # Note that relative paths are relative to the directory from which doxygen is
 # run.
 
-EXCLUDE                = ./src/shared/utils/testutils
+EXCLUDE                = ./src/shared/utils/TestUtils
 
 # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
 # directories that are symbolic links (a Unix file system feature) are excluded
diff --git a/libs/mavlink-skyward-lib b/libs/mavlink-skyward-lib
new file mode 160000
index 0000000000000000000000000000000000000000..94b57a3b5d5c200e69817066d69bc2e37cf963af
--- /dev/null
+++ b/libs/mavlink-skyward-lib
@@ -0,0 +1 @@
+Subproject commit 94b57a3b5d5c200e69817066d69bc2e37cf963af
diff --git a/libs/mavlink_skyward_lib b/libs/mavlink_skyward_lib
deleted file mode 160000
index e81ebd93ca06167e672486b944334a7b5b34226b..0000000000000000000000000000000000000000
--- a/libs/mavlink_skyward_lib
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit e81ebd93ca06167e672486b944334a7b5b34226b
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index e213d17f3ddfb6d5ae223038f5162b387518a501..441aca88b44e5438e04abbef1639cd7f9df7bb37 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit e213d17f3ddfb6d5ae223038f5162b387518a501
+Subproject commit 441aca88b44e5438e04abbef1639cd7f9df7bb37
diff --git a/libs/mxgui b/libs/mxgui
index a3f44e0cc0c7965f947b18621470d22fcabddc7b..0052374bc85c2bf90753faba61ecc08905658c83 160000
--- a/libs/mxgui
+++ b/libs/mxgui
@@ -1 +1 @@
-Subproject commit a3f44e0cc0c7965f947b18621470d22fcabddc7b
+Subproject commit 0052374bc85c2bf90753faba61ecc08905658c83
diff --git a/libs/tscpp b/libs/tscpp
index c8674a48e794c246f79c56b163ebab72d3654846..935bf1944e1ddeaf51fd22cdce04a2a97762890f 160000
--- a/libs/tscpp
+++ b/libs/tscpp
@@ -1 +1 @@
-Subproject commit c8674a48e794c246f79c56b163ebab72d3654846
+Subproject commit 935bf1944e1ddeaf51fd22cdce04a2a97762890f
diff --git a/src/shared/math/Matrix.cpp b/old_examples/shared/math/Matrix.cpp
similarity index 100%
rename from src/shared/math/Matrix.cpp
rename to old_examples/shared/math/Matrix.cpp
diff --git a/src/shared/math/Matrix.h b/old_examples/shared/math/Matrix.h
similarity index 100%
rename from src/shared/math/Matrix.h
rename to old_examples/shared/math/Matrix.h
diff --git a/src/shared/math/Vec3.h b/old_examples/shared/math/Vec3.h
similarity index 100%
rename from src/shared/math/Vec3.h
rename to old_examples/shared/math/Vec3.h
diff --git a/old_examples/tests/catch/example-test-fsm.cpp b/old_examples/tests/catch/example-test-fsm.cpp
index 5c79a22446c0506fea3c8c91c6de62132d612a25..9c69e2b3000ca19754a076f3c52b379bb12e66b1 100644
--- a/old_examples/tests/catch/example-test-fsm.cpp
+++ b/old_examples/tests/catch/example-test-fsm.cpp
@@ -34,7 +34,7 @@
 #define protected public
 
 #include <miosix.h>
-#include <utils/testutils/TestHelper.h>
+#include <utils/TestUtils/TestHelper.h>
 
 #include <catch2/catch.hpp>
 
@@ -131,7 +131,7 @@ public:
     // Start the thread in the constructor
     FSMTestFixture()
     {
-        sEventBroker.start();
+        EventBroker::getInstance().start();
         fsm.start();
     }
     // Stop the thread in the destructor
@@ -168,7 +168,7 @@ TEST_CASE_METHOD(FSMTestFixture, "Testing async transitions")
 
         SECTION("S3 --> S4 if EV_B. Delayed event should be removed")
         {
-            EventCounter counter{*sEventBroker};
+            EventCounter counter{*EventBroker::getInstance()};
 
             // Post EV_B before EV_D fires. should move to S4 and remove EV_D
             // from the delayed events
diff --git a/old_examples/tests/catch/example-test-fsm.h b/old_examples/tests/catch/example-test-fsm.h
index 3aa648ec901654b0a6dbfaf836981f1d66e72e81..03f46222db911c7304992b33a1ee525771567b5e 100644
--- a/old_examples/tests/catch/example-test-fsm.h
+++ b/old_examples/tests/catch/example-test-fsm.h
@@ -39,8 +39,8 @@
  */
 enum ExampleEvents : uint8_t
 {
-    EV_A = EV_FIRST_SIGNAL,  // The first event must always have value
-                             // EV_FIRST_SIGNAL
+    EV_A = EV_FIRST_CUSTOM,  // The first event must always have value
+                             // EV_FIRST_CUSTOM
     EV_B,  // Values for the following event can be manually specified or
            // assigned automatically
     EV_C,
@@ -110,10 +110,10 @@ public:
     FSMExample() : FSM(&FSMExample::state_S1), v(0)
     {
         // Subscribe for events posted on TOPIC_T1
-        sEventBroker.subscribe(this, TOPIC_T1);
+        EventBroker::getInstance().subscribe(this, TOPIC_T1);
     }
 
-    ~FSMExample() { sEventBroker.unsubscribe(this); }
+    ~FSMExample() { EventBroker::getInstance().unsubscribe(this); }
 
     /*
      * State function definitions.
@@ -207,8 +207,8 @@ public:
                 v = 1;
 
                 // Post EV_D to itself in 1 seconds
-                delayed_ev_id =
-                    sEventBroker.postDelayed(Event{EV_D}, TOPIC_T1, 1000);
+                delayed_ev_id = EventBroker::getInstance().postDelayed(
+                    Event{EV_D}, TOPIC_T1, 1000);
 
                 break;
             }
@@ -219,7 +219,7 @@ public:
                 // don't remove a delayed event. This is an error! Uncomment to
                 // fix, see the wiki for further information !!!
 
-                // sEventBroker.removeDelayed(delayed_ev_id);
+                // EventBroker::getInstance().removeDelayed(delayed_ev_id);
                 break;
             }
             case EV_B:
diff --git a/old_examples/tests/catch/misc/xbee-bitrate.cpp b/old_examples/tests/catch/misc/xbee-bitrate.cpp
index 6bf877b9ee87a832215351ffbd0d65b2b7bbf44d..47f40a10fce5ea70e4060026333041251e2c4db4 100644
--- a/old_examples/tests/catch/misc/xbee-bitrate.cpp
+++ b/old_examples/tests/catch/misc/xbee-bitrate.cpp
@@ -22,9 +22,9 @@
 
 #include <drivers/spi/SPIDriver.h>
 #include <interfaces-impl/hwmapping.h>
-#include <math/Stats.h>
 #include <miosix.h>
 #include <radio/Xbee/Xbee.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdio>
 #include <iostream>
diff --git a/old_examples/tests/catch/misc/xbee-send-rcv.cpp b/old_examples/tests/catch/misc/xbee-send-rcv.cpp
index cba700960f8e653a164f6dc9394851a5c00b5044..87c8c8335961362424518bfb8cdf842bca50d559 100644
--- a/old_examples/tests/catch/misc/xbee-send-rcv.cpp
+++ b/old_examples/tests/catch/misc/xbee-send-rcv.cpp
@@ -22,9 +22,9 @@
 
 #include <drivers/spi/SPIDriver.h>
 #include <interfaces-impl/hwmapping.h>
-#include <math/Stats.h>
 #include <miosix.h>
 #include <radio/Xbee/Xbee.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdio>
 #include <iostream>
diff --git a/old_examples/tests/catch/misc/xbee-time-to-send.cpp b/old_examples/tests/catch/misc/xbee-time-to-send.cpp
index c915459d6b41fb1446239b49d76dfbf70485cd0a..eb08047f3087a7fe5b9aa00aaaecaaf006836e79 100644
--- a/old_examples/tests/catch/misc/xbee-time-to-send.cpp
+++ b/old_examples/tests/catch/misc/xbee-time-to-send.cpp
@@ -22,9 +22,9 @@
 
 #include <drivers/BusTemplate.h>
 #include <interfaces-impl/hwmapping.h>
-#include <math/Stats.h>
 #include <miosix.h>
 #include <radio/Xbee/Xbee.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdio>
 #include <iostream>
diff --git a/old_examples/tests/catch/test-xbee.cpp b/old_examples/tests/catch/test-xbee.cpp
index 9973a4db02411e48f2acc8bc9f962d1c22493544..3ec81dfba6464ce558d983edec75abb2cbde0d2c 100644
--- a/old_examples/tests/catch/test-xbee.cpp
+++ b/old_examples/tests/catch/test-xbee.cpp
@@ -30,7 +30,7 @@
 #define private public
 
 #include <radio/Xbee/Xbee.h>
-#include <utils/testutils/BusTemplateMock.h>
+#include <utils/TestUtils/BusTemplateMock.h>
 
 using std::vector;
 
diff --git a/old_examples/tests/drivers/test-imu-adis.cpp b/old_examples/tests/drivers/test-imu-adis.cpp
index e6f5e31ac86b92e29bd1a45348605d402464447d..777c33d8f60da0d7f638fdaaf3ec229f54af6a4e 100644
--- a/old_examples/tests/drivers/test-imu-adis.cpp
+++ b/old_examples/tests/drivers/test-imu-adis.cpp
@@ -25,9 +25,9 @@
 #include <drivers/BusTemplate.h>
 #include <drivers/spi/SensorSpi.h>
 #include <interfaces-impl/hwmapping.h>
-#include <math/Stats.h>
 #include <sensors/ADIS16405/ADIS16405.h>
 #include <sensors/SensorSampler.h>
+#include <utils/Stats/Stats.h>
 
 using namespace miosix;
 using namespace miosix::interfaces;
diff --git a/old_examples/tests/drivers/test-lsm.cpp b/old_examples/tests/drivers/test-lsm.cpp
index 5aec0b6344c8db38d24387512b62cc3d7d5a7fb2..6f0dbc430be8d67b25d945007e6c9519df8647b5 100644
--- a/old_examples/tests/drivers/test-lsm.cpp
+++ b/old_examples/tests/drivers/test-lsm.cpp
@@ -25,9 +25,9 @@
 #include <drivers/BusTemplate.h>
 #include <drivers/spi/SensorSpi.h>
 #include <interfaces-impl/hwmapping.h>
-#include <math/Stats.h>
 #include <sensors/LSM6DS3H/LSM6DS3H.h>
 #include <sensors/SensorSampler.h>
+#include <utils/Stats/Stats.h>
 
 using namespace miosix;
 using namespace miosix::interfaces;
diff --git a/scripts/generators/eventgen.py b/scripts/generators/eventgen.py
index dedf167d77171b073d56623f605f9139855b4010..f5014d9517d50e1ee6b3db965868af308db73ffc 100755
--- a/scripts/generators/eventgen.py
+++ b/scripts/generators/eventgen.py
@@ -93,7 +93,7 @@ def generate_events_h(events, date):
 
     # Prepare the event list
     enum_event_list = list(events)
-    enum_event_list[0] += ' = EV_FIRST_SIGNAL'
+    enum_event_list[0] += ' = EV_FIRST_CUSTOM'
     enum_event_list = '\n'.join(
         [' '*4 + event + ',' for event in enum_event_list])
     vector_event_list = '\n'.join([' '*4 + event + ',' for event in events])
diff --git a/scripts/generators/fsmgen.py b/scripts/generators/fsmgen.py
index b3c6bab216cb027497e6c5c3fd4129f91b583ba7..5288d3618a19e29653f97d4a7ce26e8bc5bc649e 100755
--- a/scripts/generators/fsmgen.py
+++ b/scripts/generators/fsmgen.py
@@ -59,7 +59,7 @@ STATE_FUNCTION_CASE_TEMPLATE = ' '*8 + 'case {event}:\n' + \
     ' '*12 + 'break;\n' + \
     ' '*8 + '}}'
 UTILITY_FUNCTION_DECLARATION_TEMPLATE = 'void {function_name}();'
-TOPICS_SUBSCIPTION_TEMPLATE = ' '*4 + 'sEventBroker.subscribe(this, {topic});'
+TOPICS_SUBSCIPTION_TEMPLATE = ' '*4 + 'EventBroker::getInstance().subscribe(this, {topic});'
 UTILITY_FUNCTION_DEFINITION_TEMPLATE = 'void {state_machine_name}Controller::{function_name}()\n{{\n    // ...\n}}'
 STATE_FUNCTION_CASE_TRANSITION_TO_TARGET_TEMPLATE = 'transition(&{state_machine_name}Controller::state_{state_name});'
 TEST_CASE_METHOD_TEMPLATE = 'TEST_CASE_METHOD({state_machine_name}ControllerFixture, "Testing transitions from {state}")\n' + \
diff --git a/scripts/generators/templates/FSMController.cpp.template b/scripts/generators/templates/FSMController.cpp.template
index 583fb435c4bdb89414a829df229a87349a374155..2ea5f355f54898bee5e1eb88647bcb87b97ac4e9 100644
--- a/scripts/generators/templates/FSMController.cpp.template
+++ b/scripts/generators/templates/FSMController.cpp.template
@@ -41,7 +41,7 @@ using namespace {state_machine_name}Config;
 
 {state_machine_name}Controller::~{state_machine_name}Controller()
 {{
-    sEventBroker.unsubscribe(this);
+    EventBroker::getInstance().unsubscribe(this);
 }}
 
 {states_functions_definitions}
diff --git a/scripts/generators/templates/test.cpp.template b/scripts/generators/templates/test.cpp.template
index 73a93088404afceda5b285fa48429ce3e88172d0..2eb55502e2ed85421c8bed9310b86efd1e715f86 100644
--- a/scripts/generators/templates/test.cpp.template
+++ b/scripts/generators/templates/test.cpp.template
@@ -30,11 +30,11 @@
 
 #include <miosix.h>
 
-#include <utils/testutils/catch.hpp>
+#include <utils/TestUtils/catch.hpp>
 
 #include "{state_machine_name}Controller/{state_machine_name}Controller.h"
 #include <events/Events.h>
-#include <utils/testutils/TestHelper.h>
+#include <utils/TestUtils/TestHelper.h>
 
 using miosix::Thread;
 using namespace DeathStackBoard;
@@ -46,7 +46,7 @@ public:
     {state_machine_name}ControllerFixture()
     {{
         controller = new {state_machine_name}Controller();
-        sEventBroker.start();
+        EventBroker::getInstance().start();
         controller->start();
     }}
 
@@ -54,8 +54,8 @@ public:
     ~{state_machine_name}ControllerFixture()
     {{
         controller->stop();
-        sEventBroker.unsubscribe(controller);
-        sEventBroker.clearDelayedEvents();
+        EventBroker::getInstance().unsubscribe(controller);
+        EventBroker::getInstance().clearDelayedEvents();
         delete controller;
     }}
 
diff --git a/scripts/logdecoder/Makefile b/scripts/logdecoder/Makefile
index 0a30ee85eaf9786bc68f52dcd335486e4766392e..4f9857ad3fe80b1640b317a05abcedc6a3791dd7 100644
--- a/scripts/logdecoder/Makefile
+++ b/scripts/logdecoder/Makefile
@@ -1,6 +1,12 @@
+BOARDCORE := ../../
 
 all:
-	g++ -std=c++11 -O2 -o logdecoder logdecoder.cpp ../libs/tscpp/stream.cpp  -I ../libs
-
+	g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \
+					-DCOMPILE_FOR_X86 \
+					$(BOARDCORE)libs/tscpp/tscpp/stream.cpp \
+	 				-I$(BOARDCORE)libs/mavlink-skyward-lib \
+	 				-I$(BOARDCORE)libs/eigen \
+	 				-I$(BOARDCORE)libs/tscpp \
+	 				-I$(BOARDCORE)src/shared
 clean:
 	rm logdecoder
diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp
index 96b81e338727137ff3d25d077d1c56891226fd15..8227f53788003dcd68bbef6a6860a4bccc1977fd 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -1,59 +1,109 @@
-/***************************************************************************
- *   Copyright (C) 2018 by Terraneo Federico                               *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   As a special exception, if other files instantiate templates or use   *
- *   macros or inline functions from this file, or you compile this file   *
- *   and link it with other works to produce a work based on this file,    *
- *   this file does not by itself cause the resulting work to be covered   *
- *   by the GNU General Public License. However the source code for this   *
- *   file must still be made available in accordance with the GNU General  *
- *   Public License. This exception does not invalidate any other reasons  *
- *   why a work based on this file might be covered by the GNU General     *
- *   Public License.                                                       *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, see <http://www.gnu.org/licenses/>   *
- ***************************************************************************/
-
-/*
- * This is a stub program for the program that will decode the logged data.
- * Fill in the TODO to make it work.
+/* Copyright (c) 2018-2022 Skyward Experimental Rocketry
+ * Authors: Terrane Federico
+ *
+ * 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 <iostream>
+#include <logger/Deserializer.h>
+#include <logger/LogTypes.h>
+#include <tscpp/stream.h>
+
 #include <fstream>
+#include <iostream>
 #include <stdexcept>
-#include <tscpp/stream.h>
+#include <string>
 
-//TODO: add here include files of serialized classes
-#include "../src/shared/logger/LogStats.h"
-#include "../src/entrypoints/test_logger.h"
+/**
+ * @brief Binary log files decoder.
+ *
+ * This program is to compile for you computer and decodes binary log files
+ * through the tscpp library.
+ *
+ * In LogTypes.h there should be included all the classes you want to
+ * deserialize.
+ */
 
-using namespace std;
 using namespace tscpp;
+using namespace Boardcore;
+
+void showUsage(const string& cmdName)
+{
+    std::cerr << "Usage: " << cmdName << " {-a | <log_file_name> | -h}"
+              << "Options:\n"
+              << "\t-h,--help\t\tShow help message\n"
+              << "\t-a,--all Deserialize all logs in the current directory\n"
+              << std::endl;
+}
+
+bool deserialize(string logName)
+{
+    std::cout << "Deserializing " << logName << "...\n";
+    Deserializer d(logName);
+    LogTypes::registerTypes(d);
+
+    return d.deserialize();
+}
+
+bool deserializeAll()
+{
+    for (int i = 0; i < 100; i++)
+    {
+        char nextName[11];
+        sprintf(nextName, "log%02d.dat", i);
+        struct stat st;
+        if (stat(nextName, &st) != 0)
+            continue;  // File not found
+        // File found
+        if (!deserialize(string(nextName)))
+            return false;
+    }
+    return true;
+}
+
+int main(int argc, char* argv[])
+{
+    if (argc < 2)
+    {
+        showUsage(string(argv[0]));
+        return 1;  // Error
+    }
+
+    bool success = false;
+    string arg1  = string(argv[1]);
+
+    // Help message
+    if (arg1 == "-h" || arg1 == "--help")
+    {
+        showUsage(string(argv[0]));
+        return 0;
+    }
+
+    // File deserialization
+    if (arg1 == "-a" || arg1 == "--all")
+        success = deserializeAll();
+    else
+        success = deserialize(arg1);
 
-int main(int argc, char *argv[])
-try {
-    TypePoolStream tp;
-    //TODO: Register the serialized classes
-    tp.registerType<LogStats>([](LogStats& t){ t.print(cout); cout<<'\n'; });
-    tp.registerType<Dummy>([](Dummy& t){ t.print(cout); cout<<'\n'; });
-
-    if(argc!=2) return 1;
-    ifstream in(argv[1]);
-    in.exceptions(ios::eofbit);
-    UnknownInputArchive ia(in,tp);
-    for(;;) ia.unserialize();
-} catch(exception&) {
+    // End
+    if (success)
+        std::cout << "Deserialization completed successfully\n";
+    else
+        std::cout << "Deserialization ended with errors\n";
     return 0;
 }
diff --git a/scripts/logdecoder/sample_no_cereal.dat.dat b/scripts/logdecoder/sample_no_cereal.dat.dat
deleted file mode 100644
index 23dff819fee59674f0cfff237b94fffa191d4a06..0000000000000000000000000000000000000000
Binary files a/scripts/logdecoder/sample_no_cereal.dat.dat and /dev/null differ
diff --git a/scripts/logdecoder/xbee/.gitignore b/scripts/logdecoder/xbee/.gitignore
deleted file mode 100644
index 3e6fd163051d66dceb9c808d07e314ae4d022ee5..0000000000000000000000000000000000000000
--- a/scripts/logdecoder/xbee/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-logdecoder
diff --git a/scripts/logdecoder/xbee/LogTypes.h b/scripts/logdecoder/xbee/LogTypes.h
deleted file mode 100644
index cb82189b489a302f28b3f62dfd61fec88c1326b7..0000000000000000000000000000000000000000
--- a/scripts/logdecoder/xbee/LogTypes.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/* Copyright (c) 2015-2018 Skyward Experimental Rocketry
- * Author: Luca Erbetta
- *
- * 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 <logger/Deserializer.h>
-#include <logger/LogStats.h>
-#include <radio/Xbee/APIFramesLog.h>
-#include <radio/Xbee/Mark.h>
-#include <radio/Xbee/XbeeStatus.h>
-#include <radio/Xbee/XbeeTestData.h>
-
-#include <fstream>
-#include <iostream>
-
-// Serialized classes
-using std::ofstream;
-
-namespace Boardcore
-{
-
-template <typename T>
-void print(T& t, ostream& os)
-{
-    t.print(os);
-}
-
-template <typename T>
-void registerType(Deserializer& ds)
-{
-    ds.registerType<T>(print<T>, T::header());
-}
-
-void registerTypes(Deserializer& ds)
-{
-    registerType<LogStats>(ds);
-
-    registerType<Xbee::APIFrameLog>(ds);
-    registerType<Xbee::ATCommandFrameLog>(ds);
-    registerType<Xbee::ATCommandResponseFrameLog>(ds);
-    registerType<Xbee::ModemStatusFrameLog>(ds);
-    registerType<Xbee::TXRequestFrameLog>(ds);
-    registerType<Xbee::TXStatusFrameLog>(ds);
-    registerType<Xbee::RXPacketFrameLog>(ds);
-    registerType<Xbee::XbeeStatus>(ds);
-
-    registerType<TxData>(ds);
-    registerType<RxData>(ds);
-    registerType<Mark>(ds);
-    registerType<XbeeConfig>(ds);
-    registerType<EnergyScanData>(ds);
-}
-
-}  // namespace Boardcore
diff --git a/scripts/logdecoder/xbee/Makefile b/scripts/logdecoder/xbee/Makefile
deleted file mode 100644
index 47258b629723d159885432ac1e485023f5d40002..0000000000000000000000000000000000000000
--- a/scripts/logdecoder/xbee/Makefile
+++ /dev/null
@@ -1,13 +0,0 @@
-BASE := ../../../../../
-
-all:
-	g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \
-					-DCOMPILE_FOR_X86 \
-					$(BASE)libs/tscpp/stream.cpp \
-	 				-I$(BASE)src/shared \
-	 				-I$(BASE)src/tests \
-					-I$(BASE)libs \
-					-I$(BASE)libs/miosix-kernel/miosix
-
-clean:
-	rm logdecoder
diff --git a/scripts/logdecoder/xbee/logdecoder.cpp b/scripts/logdecoder/xbee/logdecoder.cpp
deleted file mode 100644
index 40577eef2088983028f356bb13b341f97641ed3d..0000000000000000000000000000000000000000
--- a/scripts/logdecoder/xbee/logdecoder.cpp
+++ /dev/null
@@ -1,168 +0,0 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Author: Federico Terraneo
- *
- * 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.
- */
-
-/*
- * This is a stub program for the program that will decode the logged data.
- * You must first define a function
- *  "void registerTypes(Deserializer& ds);"
- *
- * that registers all the required log data types in the deserializer and then
- * INCLUDE this cpp file in your source. finally compile and run
- */
-
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <tscpp/stream.h>
-
-#include <filesystem>
-#include <fstream>
-#include <iostream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-
-#include "LogTypes.h"
-
-using namespace std;
-using namespace tscpp;
-
-namespace fs = filesystem;
-
-void showHelp(string cmdName)
-{
-    std::cerr << "Usage: " << cmdName
-              << " {-a [logs_diretory] | <log_file_path> | -h}"
-              << "Options:\n"
-              << "\t-h,--help\t\tShow help message\n"
-              << "\t-a,--all [dir=\".\"] Deserialize all logs in the provided "
-                 "directory\n"
-              << std::endl;
-}
-
-vector<fs::path> listLogFiles(fs::path dir)
-{
-    vector<fs::path> out;
-    for (const auto& entry : fs::directory_iterator(dir))
-    {
-        if (entry.exists() && entry.is_regular_file())
-        {
-            if (entry.path().extension() == ".dat")
-            {
-                out.push_back(entry.path());
-            }
-        }
-    }
-    return out;
-}
-
-bool deserialize(fs::path log_path)
-{
-    cout << "Deserializing " << log_path << ".dat...\n";
-
-    // remove extension
-    log_path.replace_extension("");
-    Deserializer d(log_path);
-    registerTypes(d);
-
-    return d.deserialize();
-}
-
-bool deserializeAll(fs::path dir = ".")
-{
-    vector<fs::path> logs = listLogFiles(dir);
-    for (auto log : logs)
-    {
-        if (!deserialize(log.string()))
-        {
-            return false;
-        }
-    }
-
-    return true;
-}
-
-int main(int argc, char* argv[])
-{
-    if (argc < 2)
-    {
-        showHelp(string(argv[0]));
-        return 1;
-    }
-
-    bool success = false;
-    string arg1  = string(argv[1]);
-    if (arg1 == "-h" || arg1 == "--help")
-    {
-        showHelp(string(argv[0]));
-        return 0;
-    }
-
-    if (arg1 == "-a" || arg1 == "--all")
-    {
-        fs::path dir = ".";
-        if (argc == 3)
-        {
-            string arg2 = string(argv[2]);
-            fs::directory_entry entry(arg2);
-            if (entry.exists() && entry.is_directory())
-            {
-                dir = arg2;
-            }
-            else
-            {
-                cout << "Second argument after '-a' or '--all' must be a "
-                        "directory\n";
-                showHelp(string(argv[0]));
-                return 1;
-            }
-        }
-        cout << "Deserializing all logs...\n";
-        success = deserializeAll(dir);
-    }
-    else if (arg1[0] == '-')
-    {
-        cerr << "Unknown option\n";
-        return 1;
-    }
-    else
-    {
-        fs::directory_entry entry(arg1);
-        if (entry.exists() && entry.is_regular_file())
-        {
-            success = deserialize(arg1);
-        }
-        else
-        {
-            showHelp(string(argv[0]));
-            return 1;
-        }
-    }
-
-    if (success)
-    {
-        cout << "Deserialization completed successfully.\n";
-    }
-    else
-    {
-        cout << "Deserialization ended with errors.\n";
-    }
-}
diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index 0124a2310951b7803879e9fce2c79466ba04880f..d2aa4746b97f41ee1b47c5306c6ea884f1f859ef 100644
--- a/src/entrypoints/bmx160-calibration-entry.cpp
+++ b/src/entrypoints/bmx160-calibration-entry.cpp
@@ -24,7 +24,8 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <sensors/BMX160/BMX160.h>
 #include <sensors/BMX160/BMX160WithCorrection.h>
-#include <sensors/calibration/SoftIronCalibration.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
 #include <utils/Debug.h>
 
 #include <fstream>
@@ -41,9 +42,9 @@ constexpr int ACC_CALIBRATION_SLEEP_TIME     = 25;  // [us]
 constexpr int ACC_CALIBRATION_N_ORIENTATIONS = 6;
 
 constexpr int MAG_CALIBRATION_SAMPLES  = 500;
-constexpr int MAG_CALIBRATION_DURAITON = 60;  // [s]
+constexpr int MAG_CALIBRATION_DURATION = 60;  // [s]
 constexpr int MAG_CALIBRATION_SLEEP_TIME =
-    MAG_CALIBRATION_DURAITON * 1000 / MAG_CALIBRATION_SAMPLES;  // [us]
+    MAG_CALIBRATION_DURATION * 1000 / MAG_CALIBRATION_SAMPLES;  // [us]
 
 BMX160* bmx160;
 
@@ -330,7 +331,7 @@ BMX160CorrectionParameters calibrateMagnetometer(
     BMX160CorrectionParameters correctionParameters)
 {
     Eigen::Matrix<float, 3, 2> m;
-    auto* calibrationModel = new SoftIronCalibration<MAG_CALIBRATION_SAMPLES>;
+    auto* calibrationModel = new SoftAndHardIronCalibration;
     Eigen::Vector3f avgMag{0, 0, 0}, vec;
 
     SPIBus bus(SPI1);
@@ -389,7 +390,7 @@ BMX160CorrectionParameters calibrateMagnetometer(
         "the "
         "most different directions.\n");
     printf("The calibration will run for %d seconds\n",
-           MAG_CALIBRATION_DURAITON);
+           MAG_CALIBRATION_DURATION);
 
     if (!askToContinue())
     {
@@ -412,26 +413,6 @@ BMX160CorrectionParameters calibrateMagnetometer(
     printf("Computing the result....");
     auto corrector = calibrationModel->computeResult();
 
-    // Save the calibration data in the sd card
-    // Save the correction parameters in the file
-    {
-        std::ofstream magnetometerCalibrationDataFile(
-            MAG_CALIBRATION_DATA_FILE);
-
-        auto samples        = calibrationModel->getSamples();
-        int numberOfSamples = samples.rows();
-
-        for (int i = 0; i < numberOfSamples; i++)
-        {
-            auto row = samples.row(i);
-            for (int j = 0; j < row.cols(); j++)
-            {
-                magnetometerCalibrationDataFile << samples.row(i)(j) << ",";
-            }
-            magnetometerCalibrationDataFile << "\n";
-        }
-    }
-
     corrector >> m;
     corrector >> correctionParameters.magnetoParams;
 
diff --git a/src/entrypoints/imu-calibration.cpp b/src/entrypoints/imu-calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c681ea0eaf784188bceea4c47284471bf1e220d
--- /dev/null
+++ b/src/entrypoints/imu-calibration.cpp
@@ -0,0 +1,131 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+#include <utils/Debug.h>
+
+#include <iostream>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace miosix;
+
+int menu();
+bool askToContinue();
+
+void calibrateMagnetometer();
+
+constexpr int MAG_CALIBRATION_DURATION = 30;  // [s]
+constexpr int IMU_SAMPLE_PERIOD        = 10;  // [ms] 100Hz
+
+int main()
+{
+    // Greet the user
+    printf("\nWelcome to the calibration procedure!\n");
+
+    // Make the user choose what to do
+    switch (menu())
+    {
+        case 2:
+            calibrateMagnetometer();
+            break;
+
+        default:
+            break;
+    }
+
+    printf("end\n");
+    reboot();
+}
+
+int menu()
+{
+    int choice;
+
+    printf("\nWhat do you want to do?\n");
+    printf("1. Calibrate accelerometer\n");
+    printf("2. Calibrate magnetometer\n");
+    printf("3. Set minimum gyroscope samples for calibration\n");
+    printf("\n>> ");
+    scanf("%d", &choice);
+
+    return choice;
+}
+
+bool askToContinue()
+{
+    char c;
+
+    std::cout << "Write 'c' to continue, otherwise stop:\n";
+    scanf("%c", &c);
+
+    return c != 'c';
+}
+
+void calibrateMagnetometer()
+{
+    SPIBus spiBus(SPI1);
+    MPU9250 mpu(spiBus, sensors::mpu9250::cs::getPin());
+    mpu.init();
+
+    printf("Now the magnetometer calibration will begin\n");
+    printf("Please, rotate the gyroscope A LOT!\n");
+    printf("The calibration will run for %d seconds\n",
+           MAG_CALIBRATION_DURATION);
+
+    // if (!askToContinue())
+    //     return;
+    printf("Starting...\n");
+
+    // Prepare the calibration model
+    SoftAndHardIronCalibration calibration;
+
+    // Prepare and start the sensor manager
+    TaskScheduler scheduler;
+    scheduler.addTask([]() { printf("...\n"); }, 500);
+    scheduler.start();
+
+    // Wait and then stop the sampling
+    auto startTick = getTick();
+    auto lastTick  = startTick;
+    while (getTick() - startTick < MAG_CALIBRATION_DURATION * 1e3)
+    {
+        mpu.sample();
+        calibration.feed(mpu.getLastSample());
+        Thread::sleepUntil(lastTick + IMU_SAMPLE_PERIOD);
+        lastTick = getTick();
+    }
+
+    scheduler.stop();
+
+    printf("Computing the result...\n");
+
+    auto correction = calibration.computeResult();
+
+    printf("b: the bias vector\n");
+    std::cout << correction.getOffset().transpose() << std::endl;
+    printf("g: the gain to be multiplied to the input vector\n");
+    std::cout << correction.getGain().transpose() << std::endl;
+}
diff --git a/src/entrypoints/kernel-testsuite.cpp b/src/entrypoints/kernel-testsuite.cpp
index 97cf173c2cf7e6434d56b635335c4bc94d18326c..89c3260272c61d4c0e2e4247573fc97362176ce3 100644
--- a/src/entrypoints/kernel-testsuite.cpp
+++ b/src/entrypoints/kernel-testsuite.cpp
@@ -426,8 +426,8 @@ void process_test_file_concurrency()
             fail("Wrong data from file 2");
     }
 
-    fclose(f1);  // cppcheck-suppress nullPointerRedundantCheck
-    fclose(f2);  // cppcheck-suppress nullPointerRedundantCheck
+    fclose(f1);
+    fclose(f2);
 
     pass();
 }
@@ -584,7 +584,7 @@ void syscall_test_files()
             fail("Open with O_RDWR should have failed, the file doesn't exist");
             break;
         case 2:
-            fail("Cannot craete new file");
+            fail("Cannot create new file");
             break;
         case 3:
             fail("file descriptor not valid");
@@ -596,7 +596,7 @@ void syscall_test_files()
             fail("read has read the wrong amount of data");
             break;
         case 6:
-            fail("readed data doesn't match the written one");
+            fail("read data doesn't match the written one");
             break;
         case 7:
             fail("close hasn't returned 0");
@@ -605,10 +605,10 @@ void syscall_test_files()
             fail("open with O_RDWR failed, but the file exists");
             break;
         case 9:
-            fail("read has return the wrogn amount of data");
+            fail("read has return the wrong amount of data");
             break;
         case 10:
-            fail("readed data doesn't match the written one");
+            fail("read data doesn't match the written one");
             break;
         case 11:
             fail("close hasn't returned 0");
@@ -2133,7 +2133,7 @@ static void test_9()
 // Test 10
 //
 /*
-tests exception propagation throug entry point of a thread
+tests exception propagation through entry point of a thread
 */
 
 #ifndef __NO_EXCEPTIONS
@@ -4171,7 +4171,7 @@ static void test_24()
     checkAtomicOps<Derived1, Derived1>();
     checkAtomicOps<Derived2, Derived2>();
 
-    // atomic_load(), atomic_store(), with polimorphism
+    // atomic_load(), atomic_store(), with polymorphism
     checkAtomicOps<Base0, Derived0>();
     checkAtomicOps<Middle1, Derived1>();
     checkAtomicOps<Middle1, Derived2>();
@@ -4530,6 +4530,7 @@ static void fs_test_1()
     i = 0;
     for (;;)
     {
+        // cppcheck-suppress nullPointerRedundantCheck
         j = fread(buf, 1, 1024, f);
         if (j == 0)
             break;
@@ -4549,6 +4550,7 @@ static void fs_test_1()
     i = 0;
     for (;;)
     {
+        // cppcheck-suppress nullPointerRedundantCheck
         j = fread(buf, 1, 1024, f);
         if (j == 0)
             break;
@@ -4568,6 +4570,7 @@ static void fs_test_1()
     i = 0;
     for (;;)
     {
+        // cppcheck-suppress nullPointerRedundantCheck
         j = fread(buf, 1, 1024, f);
         if (j == 0)
             break;
@@ -4592,7 +4595,10 @@ static void fs_test_1()
     if ((f = fopen("/sd/testdir/file_4.txt", "a")) == NULL)
         fail("can't open a file_4.txt");
     for (i = 2; i <= 128; i++)
+    {
+        // cppcheck-suppress nullPointerRedundantCheck
         fprintf(f, "Hello world line %03d\n", i);
+    }
     if (fclose(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
         fail("Can't close a file_4.txt");
     // Reading to check (only first 2 lines)
@@ -4603,11 +4609,10 @@ static void fs_test_1()
     fgets(line, sizeof(line), f);
     if (strcmp(line, "Hello world line 001\n"))
         fail("file_4.txt line 1 error");
-    // cppcheck-suppress nullPointerRedundantCheck
     fgets(line, sizeof(line), f);
     if (strcmp(line, "Hello world line 002\n"))
         fail("file_4.txt line 2 error");
-    if (fclose(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
+    if (fclose(f) != 0)
         fail("Can't close r file_4.txt");
     // Test fseek and ftell. When reaching this point file_4.txt contains:
     // Hello world line 001\n
@@ -4617,10 +4622,8 @@ static void fs_test_1()
     if ((f = fopen("/sd/testdir/file_4.txt", "r")) == NULL)
         fail("can't open r2 file_4.txt");
     if (ftell(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
-        fail("File opend but cursor not @ address 0");
-    // cppcheck-suppress nullPointerRedundantCheck
+        fail("File opened but cursor not @ address 0");
     fseek(f, -4, SEEK_END);  // Seek to 128\n
-                             // cppcheck-suppress nullPointerRedundantCheck
     if ((fgetc(f) != '1') | (fgetc(f) != '2') | (fgetc(f) != '8'))
         fail("fgetc SEEK_END");
     if (ftell(f) != (21 * 128 - 1))
@@ -4628,31 +4631,25 @@ static void fs_test_1()
         iprintf("ftell=%ld\n", ftell(f));
         fail("ftell() 1");
     }
-    // cppcheck-suppress nullPointerRedundantCheck
     fseek(f, 21 + 17, SEEK_SET);  // Seek to 002\n
-    if ((fgetc(f) != '0') |       // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '0') |       // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '2') |       // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '\n'))       // cppcheck-suppress nullPointerRedundantCheck
+    if ((fgetc(f) != '0') | (fgetc(f) != '0') | (fgetc(f) != '2') |
+        (fgetc(f) != '\n'))
         fail("fgetc SEEK_SET");
-    if (ftell(f) != (21 * 2))  // cppcheck-suppress nullPointerRedundantCheck
+    if (ftell(f) != (21 * 2))
     {
         iprintf("ftell=%ld\n", ftell(f));
         fail("ftell() 2");
     }
-    // cppcheck-suppress nullPointerRedundantCheck
     fseek(f, 21 * 50 + 17, SEEK_CUR);  // Seek to 053\n
-    if ((fgetc(f) != '0') |  // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '5') |  // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '3') |  // cppcheck-suppress nullPointerRedundantCheck
-        (fgetc(f) != '\n'))  // cppcheck-suppress nullPointerRedundantCheck
+    if ((fgetc(f) != '0') | (fgetc(f) != '5') | (fgetc(f) != '3') |
+        (fgetc(f) != '\n'))
         fail("fgetc SEEK_CUR");
-    if (ftell(f) != (21 * 53))  // cppcheck-suppress nullPointerRedundantCheck
+    if (ftell(f) != (21 * 53))
     {
         iprintf("ftell=%ld\n", ftell(f));
         fail("ftell() 2");
     }
-    if (fclose(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
+    if (fclose(f) != 0)
         fail("Can't close r2 file_4.txt");
     // Testing remove()
     if ((f = fopen("/sd/testdir/deleteme.txt", "w")) == NULL)
@@ -4745,7 +4742,7 @@ static void checkInDir(const std::string &d, bool createFile)
     fgets(s, sizeof(s), f);  // cppcheck-suppress nullPointerRedundantCheck
     if (strcmp(s, teststr))
         fail("file content after rename");
-    fclose(f);  // cppcheck-suppress nullPointerRedundantCheck
+    fclose(f);
     if (unlink((d + filename2).c_str()))
         fail("unlink 3");
     if (checkDirContent(d, 0, filename2) == false)
@@ -4794,10 +4791,11 @@ static void fs_test_3()
         for (unsigned int j = 0; j < size; j++)
             buf[j] = rand() & 0xff;
         checksum ^= crc16(buf, size);
+        // cppcheck-suppress nullPointerRedundantCheck
         if (fwrite(buf, 1, size, f) != size)
             fail("write");
     }
-    if (fclose(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
+    if (fclose(f) != 0)
         fail("close 1");
 
     if ((f = fopen(name, "r")) == NULL)
@@ -4807,11 +4805,12 @@ static void fs_test_3()
     for (unsigned int i = 0; i < numBlocks; i++)
     {
         memset(buf, 0, size);
+        // cppcheck-suppress nullPointerRedundantCheck
         if (fread(buf, 1, size, f) != size)
             fail("read");
         outChecksum ^= crc16(buf, size);
     }
-    if (fclose(f) != 0)  // cppcheck-suppress nullPointerRedundantCheck
+    if (fclose(f) != 0)
         fail("close 2");
     delete[] buf;
 
@@ -5013,6 +5012,7 @@ const int nThreads = 8;
 bool flags[nThreads];
 
 static int throwable(std::vector<int> &v) __attribute__((noinline));
+// cppcheck-suppress containerOutOfBounds
 static int throwable(std::vector<int> &v) { return v.at(10); }
 
 static void test(void *argv)
@@ -5168,7 +5168,7 @@ static void benchmark_2()
 //
 /*
 tests:
-Fisesystem write speed and latency
+File system write speed and latency
 makes a 1MB file and measures time required to read/write it.
 */
 
@@ -5259,7 +5259,7 @@ quit:
 //
 /*
 tests:
-Mutex lonk/unlock time
+Mutex lock/unlock time
 */
 
 volatile bool b4_end = false;
diff --git a/src/entrypoints/sdcard-benchmark.cpp b/src/entrypoints/sdcard-benchmark.cpp
index f41ede96ff37ce124d3c97b984e867df26c839fb..89aa596440629e7fca70113320d987ad15e63467 100644
--- a/src/entrypoints/sdcard-benchmark.cpp
+++ b/src/entrypoints/sdcard-benchmark.cpp
@@ -26,8 +26,8 @@
 #include <ctime>
 #include <iostream>
 // #include <thread>
-#include <math/Stats.h>
 #include <miosix.h>
+#include <utils/Stats/Stats.h>
 
 #include <vector>
 
diff --git a/src/shared/Singleton.h b/src/shared/Singleton.h
index 5639c0b15810496bd6a8e669ff19e0ab409f1ee3..5c3975f35899c20e78d8ffd2252d9fc626754ea3 100644
--- a/src/shared/Singleton.h
+++ b/src/shared/Singleton.h
@@ -57,7 +57,7 @@ protected:
     Singleton() {}
 
 public:
-    Singleton(const Singleton&) = delete;
+    Singleton(const Singleton&)            = delete;
     Singleton& operator=(const Singleton&) = delete;
 };
 
diff --git a/src/shared/actuators/Buzzer.h b/src/shared/actuators/Buzzer.h
new file mode 100644
index 0000000000000000000000000000000000000000..50c6b33409f87870da9f18dbeb04d4050f4216dc
--- /dev/null
+++ b/src/shared/actuators/Buzzer.h
@@ -0,0 +1,174 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/timer/GeneralPurposeTimer.h>
+#include <miosix.h>
+
+namespace Boardcore
+{
+
+/**
+ * @brief This driver does not provide a square wave signal but insted is a
+ * simple utility that provides long PWM signals to make the buzzer beep
+ * on and off.
+ */
+class Buzzer
+{
+public:
+    /**
+     * Note: The timer must be a General Purpose Timer.
+     *
+     * @param timer Timer used to provide the alternating on and off signal.
+     * @param channel Timer channel to output the signal.
+     */
+    Buzzer(TIM_TypeDef *timer, TimerUtils::Channel channel);
+
+    /**
+     * @brief Turns on the buzzer.
+     *
+     * Note: Keeps the timer off.
+     */
+    void on();
+
+    /**
+     * @brief Turns off the buzzer.
+     *
+     * Note: disables and resets the timer.
+     */
+    void off();
+
+    /**
+     * @brief Turns on the buzzer for the specified amount of time.
+     *
+     * Note: This function wait for the given duration with Thread::sleep.
+     *
+     * @param ms Beep duration in milliseconds.
+     */
+    void oneTimeToggle(uint16_t ms);
+
+    /**
+     * @brief Turns on and off the buzzer indefinitely with the given timings.
+     *
+     * @param onTime Buzzer on time [ms].
+     * @param offTime Buzzer off time [ms].
+     */
+    void continuoslyToggle(uint16_t onTime, uint16_t offTime);
+
+private:
+    GP16bitTimer timer;
+    TimerUtils::Channel channel;
+};
+
+inline Buzzer::Buzzer(TIM_TypeDef *timer, TimerUtils::Channel channel)
+    : timer(timer), channel(channel)
+{
+    // this->timer.setPrescaler(
+    //     TimerUtils::computePrescalerValue(timer, frequency * 2));
+    // this->timer.setAutoReloadRegister(1);
+    // this->timer.setOutputCompareMode(channel,
+    //                                  TimerUtils::OutputCompareMode::TOGGLE);
+    // this->timer.setCaptureCompareRegister(channel, 1);
+    // this->timer.generateUpdate();
+    // this->timer.enableCaptureCompareOutput(channel);
+    // this->timer.enableCaptureCompareComplementaryOutput(channel);
+}
+
+inline void Buzzer::on()
+{
+    // First turn off and reset the timer
+    off();
+
+    // Set the polarity of the channel to low to make the output high
+    timer.setOutputCompareMode(channel,
+                               TimerUtils::OutputCompareMode::FORCE_ACTIVE);
+    timer.setCaptureComparePolarity(
+        channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW);
+    timer.enableCaptureCompareOutput(channel);
+}
+
+inline void Buzzer::off()
+{
+    timer.disable();
+    timer.reset();
+}
+
+inline void Buzzer::oneTimeToggle(uint16_t ms)
+{
+    if (ms == 0)
+        return;
+
+    // First turn off and reset the timer
+    off();
+
+    // Set the prescaler with a 10KHz frequency
+    timer.setPrescaler(
+        TimerUtils::computePrescalerValue(timer.getTimer(), 10000));
+
+    // Make the timer reload after the specified amount of millisseconds
+    timer.setAutoReloadRegister(ms * 10);
+    timer.setCaptureCompareRegister(channel, 1);
+
+    timer.setOutputCompareMode(channel,
+                               TimerUtils::OutputCompareMode::PWM_MODE_2);
+    timer.setCaptureComparePolarity(
+        channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW);
+    timer.enableCaptureCompareOutput(channel);
+
+    // Update the configuration
+    timer.generateUpdate();
+
+    // Use One Pulse Mode to stop the timer after the first update event
+    timer.enableOnePulseMode();
+    timer.enable();
+}
+
+inline void Buzzer::continuoslyToggle(uint16_t onTime, uint16_t offTime)
+{
+    if (onTime == 0 || offTime == 0)
+        return;
+
+    // First turn off and reset the timer
+    off();
+
+    // Set the prescaler with a 10KHz frequency
+    timer.setPrescaler(
+        TimerUtils::computePrescalerValue(timer.getTimer(), 10000));
+
+    // Make the timer reload after the specified amount of millisseconds
+    timer.setAutoReloadRegister((onTime + offTime) * 10);
+    timer.setCaptureCompareRegister(channel, onTime * 10);
+
+    timer.setOutputCompareMode(channel,
+                               TimerUtils::OutputCompareMode::PWM_MODE_1);
+    timer.setCaptureComparePolarity(
+        channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW);
+    timer.enableCaptureCompareOutput(channel);
+
+    // Update the configuration
+    timer.generateUpdate();
+
+    timer.enable();
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/drivers/hbridge/HBridge.cpp b/src/shared/actuators/HBridge/HBridge.cpp
similarity index 100%
rename from src/shared/drivers/hbridge/HBridge.cpp
rename to src/shared/actuators/HBridge/HBridge.cpp
diff --git a/src/shared/drivers/hbridge/HBridge.h b/src/shared/actuators/HBridge/HBridge.h
similarity index 100%
rename from src/shared/drivers/hbridge/HBridge.h
rename to src/shared/actuators/HBridge/HBridge.h
diff --git a/src/shared/drivers/servo/Servo.cpp b/src/shared/actuators/Servo/Servo.cpp
similarity index 91%
rename from src/shared/drivers/servo/Servo.cpp
rename to src/shared/actuators/Servo/Servo.cpp
index 0c4460cd2aa5ee009750a42ded9052488abce8bd..ceffefd2963bda1cb9984ea73974b692d62df2ba 100644
--- a/src/shared/drivers/servo/Servo.cpp
+++ b/src/shared/actuators/Servo/Servo.cpp
@@ -22,6 +22,8 @@
 
 #include "Servo.h"
 
+#include <drivers/timer/TimestampTimer.h>
+
 #include "miosix.h"
 
 namespace Boardcore
@@ -97,4 +99,16 @@ float Servo::getPosition180Deg() { return getPosition() * 1800; }
 
 float Servo::getPosition360Deg() { return getPosition() * 3600; }
 
+ServoData Servo::getState()
+{
+    return {TimestampTimer::getInstance().getTimestamp(),
+
+#ifndef COMPILE_FOR_HOST
+            pwm.getTimer().getTimerNumber(), static_cast<uint8_t>(pwmChannel),
+#else
+            0, 0,
+#endif
+            getPosition()};
+}
+
 }  // namespace Boardcore
diff --git a/src/shared/drivers/servo/Servo.h b/src/shared/actuators/Servo/Servo.h
similarity index 97%
rename from src/shared/drivers/servo/Servo.h
rename to src/shared/actuators/Servo/Servo.h
index b76baba37e0cae8f9479da48886272cb637a7710..2e0a511eb412e5a52f71a4399357d927ded90d62 100644
--- a/src/shared/drivers/servo/Servo.h
+++ b/src/shared/actuators/Servo/Servo.h
@@ -24,6 +24,8 @@
 #include <drivers/timer/PWM.h>
 #endif
 
+#include "ServoData.h"
+
 #pragma once
 
 namespace Boardcore
@@ -122,6 +124,11 @@ public:
 
     float getPosition360Deg();
 
+    /**
+     * @brief Returns the current position and the current timestamp.
+     */
+    ServoData getState();
+
 private:
     // This class is not copyable!
     Servo& operator=(const Servo&) = delete;
diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h b/src/shared/actuators/Servo/ServoData.h
similarity index 64%
rename from src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h
rename to src/shared/actuators/Servo/ServoData.h
index 65242fc3082bb372fe4e8cc3b966ae901a5204b4..b4ff14d357a09257f6d9882ed6921c640fad430e 100644
--- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h
+++ b/src/shared/actuators/Servo/ServoData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Authors: Alessandro Del Duca, Luca Conterio
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,30 +22,25 @@
 
 #pragma once
 
-#include <Eigen/Dense>
+#include <ostream>
 
-namespace DeathStackBoard
+namespace Boardcore
 {
 
-// function with 2 vectors as parameters
-typedef std::function<MatrixXf(VectorXf, VectorXf)> function_2v;
-// function with 1 vector as parameter
-typedef std::function<MatrixXf(VectorXf)> function_v;
-
-struct ExtendedKalmanConfig
+struct ServoData
 {
-    uint8_t n;
-    uint8_t m;
-    uint8_t p;
-
-    Eigen::Matrix<float, 12, 12> P;
-    Eigen::Matrix<float, 6, 6> Q;
-    Vector<float, 13> x;
-
-    function_v h;
-
-    function_2v dfdx;
-    function_v dhdx;
+    uint64_t timestamp;
+    uint8_t timer;
+    uint8_t channel;
+    float position;
+
+    static std::string header() { return "timestamp,timer,channel,position\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << timer << "," << channel << "," << position
+           << "\n";
+    }
 };
 
-}  // namespace DeathStackBoard
+}  // namespace Boardcore
diff --git a/src/shared/drivers/stepper/Stepper.h b/src/shared/actuators/Stepper.h
similarity index 99%
rename from src/shared/drivers/stepper/Stepper.h
rename to src/shared/actuators/Stepper.h
index 57942567c189acce7c4824fa6cbbf059055f18ca..b08639af1075f1d3534df3fd5543f3fe0e5e6b25 100644
--- a/src/shared/drivers/stepper/Stepper.h
+++ b/src/shared/actuators/Stepper.h
@@ -24,7 +24,7 @@
 
 #include <interfaces-impl/gpio_impl.h>
 #include <interfaces/delays.h>
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
 
 namespace Boardcore
 {
@@ -261,4 +261,4 @@ inline int Stepper::getMicrosteppingValue()
     return 1;
 }
 
-}  // namespace Boardcore
\ No newline at end of file
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/Algorithm.h b/src/shared/algorithms/Algorithm.h
index 7f8ace5e3da652e594dbeee7101a2ee39e8a9b83..56d31830cdc7218ecd795ab34e003e57261a13d8 100644
--- a/src/shared/algorithms/Algorithm.h
+++ b/src/shared/algorithms/Algorithm.h
@@ -22,6 +22,9 @@
 
 #pragma once
 
+namespace Boardcore
+{
+
 class Algorithm
 {
 public:
@@ -63,3 +66,5 @@ protected:
 
     bool running = false;
 };
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/kalman/KalmanEigen.h b/src/shared/algorithms/Kalman/Kalman.h
similarity index 97%
rename from src/shared/algorithms/kalman/KalmanEigen.h
rename to src/shared/algorithms/Kalman/Kalman.h
index 214e1184e9f75f2d7835fde22f99826ce533d2c3..204311d7532230955459247fcc4ece54e2777785 100644
--- a/src/shared/algorithms/kalman/KalmanEigen.h
+++ b/src/shared/algorithms/Kalman/Kalman.h
@@ -36,7 +36,7 @@ namespace Boardcore
  *           dynamically.
  */
 template <typename t, uint8_t n, uint8_t p>
-class KalmanEigen
+class Kalman
 {
     using MatrixNN = Eigen::Matrix<t, n, n>;
     using MatrixPN = Eigen::Matrix<t, p, n>;
@@ -47,7 +47,7 @@ class KalmanEigen
 
 public:
     /**
-     * @brief Configuration struct for the KalmanEigen class.
+     * @brief Configuration struct for the Kalman class.
      */
     struct KalmanConfig
     {
@@ -63,7 +63,7 @@ public:
      * @param config configuration object containing all the initialized
      *               matrices
      */
-    KalmanEigen(const KalmanConfig& config)
+    Kalman(const KalmanConfig& config)
         : F(config.F), H(config.H), Q(config.Q), R(config.R), P(config.P),
           S(MatrixPP::Zero(p, p)), K(MatrixNP::Zero(n, p)), x(config.x)
     {
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1296b639b48483d3a0309d5136e77c5a97c5fc7e
--- /dev/null
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -0,0 +1,338 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella
+ *
+ * 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 "NAS.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace Boardcore;
+using namespace Eigen;
+
+namespace Boardcore
+{
+
+NAS::NAS(NASConfig config) : config(config)
+{
+    // Covariance setup
+    {
+        // clang-format off
+        Eigen::Matrix3f P_pos{
+            {config.P_POS, 0,            0},
+            {0,            config.P_POS, 0},
+            {0,            0,            config.P_POS_VERTICAL}
+        };
+        Eigen::Matrix3f P_vel{
+            {config.P_VEL, 0,            0},
+            {0,            config.P_VEL, 0},
+            {0,            0,            config.P_VEL_VERTICAL}
+        };
+        Eigen::Matrix3f P_att  = Matrix3f::Identity() * config.P_ATT;
+        Eigen::Matrix3f P_bias = Matrix3f::Identity() * config.P_BIAS;
+        P << P_pos,               MatrixXf::Zero(3, 9),
+            MatrixXf::Zero(3, 3), P_vel, MatrixXf::Zero(3, 6),
+            MatrixXf::Zero(3, 6),        P_att, MatrixXf::Zero(3, 3),
+            MatrixXf::Zero(3, 9),               P_bias; // cppcheck-suppress constStatement
+        // clang-format on
+    }
+
+    // GPS matrixes
+    {
+        H_gps                = Matrix<float, 4, 6>::Zero();
+        H_gps.coeffRef(0, 0) = 1;
+        H_gps.coeffRef(1, 1) = 1;
+        H_gps.coeffRef(2, 3) = 1;
+        H_gps.coeffRef(3, 4) = 1;
+        H_gps_tr             = H_gps.transpose();
+        R_gps << config.SIGMA_GPS * Matrix<float, 4, 4>::Identity();
+    }
+
+    // Magnetometer utility matrix
+    R_mag << config.SIGMA_MAG * Matrix3f::Identity();
+
+    // Other utility matrixes
+    {
+        // clang-format off
+        Q_quat << (config.SIGMA_W * config.SIGMA_W * config.T + (1.0f / 3.0f) * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T * config.T) * Matrix3f::Identity(),
+            (0.5f * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T) * Matrix3f::Identity(),
+            (0.5f * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T) * Matrix3f::Identity(), // cppcheck-suppress constStatement
+            (config.SIGMA_BETA * config.SIGMA_BETA * config.T) * Matrix3f::Identity();
+        // clang-format on
+
+        Q_lin << config.SIGMA_POS * Matrix3f::Identity(), MatrixXf::Zero(3, 3),
+            // cppcheck-suppress constStatement
+            MatrixXf::Zero(3, 3), config.SIGMA_VEL * Matrix3f::Identity();
+
+        F_lin                   = Matrix<float, 6, 6>::Identity();
+        F_lin.block<3, 3>(0, 3) = config.T * Matrix3f::Identity();
+        F_lin_tr                = F_lin.transpose();
+
+        F_quat << -Matrix3f::Identity(), -Matrix3f::Identity() * config.T,
+            Matrix3f::Zero(3, 3), Matrix3f::Identity();
+        F_quat_tr = F_quat.transpose();
+
+        G_quat << -Matrix3f::Identity(), Matrix3f::Zero(3, 3),
+            Matrix3f::Zero(3, 3), Matrix3f::Identity();
+        G_quat_tr = G_quat.transpose();
+    }
+}
+
+void NAS::predictAcc(const Vector3f& acceleration)
+{
+    Matrix3f A   = body2ned(x.block<4, 1>(IDX_QUAT, 0));
+    Vector3f pos = x.block<3, 1>(IDX_POS, 0);
+    Vector3f vel = x.block<3, 1>(IDX_VEL, 0);
+
+    // Update position by integrating the velocity
+    pos = pos + vel * config.T;
+
+    // Measured acceleration in NED frame without gravity
+    Vector3f a = A * acceleration - gravityNed;
+
+    // Update velocity by integrating the acceleration
+    vel = vel + a * config.T;
+
+    // Save the updated state
+    x.block<3, 1>(IDX_POS, 0) = pos;
+    x.block<3, 1>(IDX_VEL, 0) = vel;
+
+    // Variance propagation
+    Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+    P.block<6, 6>(0, 0)           = F_lin * Pl * F_lin_tr + Q_lin;
+}
+
+void NAS::predictGyro(const Vector3f& angularVelocity)
+{
+    Vector3f bias = x.block<3, 1>(IDX_BIAS, 0);
+    Vector4f q    = x.block<4, 1>(IDX_QUAT, 0);
+
+    Vector3f omega = angularVelocity - bias;
+    // clang-format off
+    Matrix4f omega_mat{
+        { 0.0f,      omega(2), -omega(1), omega(0)},
+        {-omega(2),  0.0f,      omega(0), omega(1)},
+        { omega(1), -omega(0),  0.0f,     omega(2)},
+        {-omega(0), -omega(1), -omega(2), 0.0f}
+    };
+    // clang-format on
+
+    // Update orientation by integrating the angular velocity
+    q = (Matrix4f::Identity() + 0.5F * omega_mat * config.T) * q;
+    q.normalize();
+
+    // Save the updated state
+    x.block<4, 1>(IDX_QUAT, 0) = q;
+
+    // Variance propagation
+    // TODO: Optimize last G_quat * Q_quat * G_att_tr
+    Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT);
+    Pq = F_quat * Pq * F_quat_tr + G_quat * Q_quat * G_quat_tr;
+    P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
+}
+
+void NAS::correctBaro(const float pressure, const float mslPress,
+                      const float mslTemp)
+{
+    Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
+
+    // Temperature at current altitude. Since in NED the altitude is negative,
+    // mslTemperature returns temperature at current altitude and not at msl
+    float temp = Aeroutils::mslTemperature(mslTemp, x(2));
+
+    // Compute gradient of the altitude-pressure function
+    H[2] = Constants::a * Constants::n * mslPress *
+           powf(1 - Constants::a * x(2) / temp, -Constants::n - 1) / temp;
+
+    Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+    Matrix<float, 1, 1> S =
+        H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_BAR);
+    Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse();
+    P.block<6, 6>(0, 0)   = (Matrix<float, 6, 6>::Identity() - K * H) * Pl;
+
+    float y_hat = Aeroutils::mslPressure(mslPress, mslTemp, x(2));
+
+    // Update the state
+    x.head<6>() = x.head<6>() + K * (pressure - y_hat);
+}
+
+void NAS::correctGPS(const Vector4f& gps)
+{
+    Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+
+    Matrix<float, 4, 4> S = H_gps * Pl * H_gps_tr + R_gps;
+    Matrix<float, 6, 4> K = Pl * H_gps_tr * S.inverse();
+
+    P.block<6, 6>(0, 0) =
+        (Eigen::Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl;
+
+    // Current state [n e vn ve]
+    Matrix<float, 4, 1> H{x(0), x(1), x(3), x(4)};
+
+    // Update the state
+    x.head<6>() = x.head<6>() + K * (gps - H);
+}
+
+void NAS::correctMag(const Vector3f& mag)
+{
+    Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
+    Matrix3f A = body2ned(q).transpose();
+
+    // Rotate the NED magnetic field in the relative reference frame
+    Vector3f mEst = A * config.NED_MAG;
+
+    // clang-format off
+    Matrix3f M{
+        { 0,      -mEst(2), mEst(1)},
+        { mEst(2), 0,      -mEst(0)},
+        {-mEst(1), mEst(0), 0}
+    };
+    // clang-format on
+
+    Matrix<float, 3, 6> H;
+    H << M, Matrix3f::Zero(3, 3);
+    Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT);
+    Matrix<float, 3, 3> S  = H * Pq * H.transpose() + R_mag;
+
+    Matrix<float, 6, 3> K  = Pq * H.transpose() * S.inverse();
+    Matrix<float, 6, 1> dx = K * (mag - mEst);
+    Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2),
+               sqrtf(1.0f - 0.25f * dx.head<3>().transpose() * dx.head<3>())};
+
+    x.block<4, 1>(IDX_QUAT, 0) = SkyQuaternion::quatProd(r, q);
+    x.block<3, 1>(IDX_BIAS, 0) += dx.tail<3>();
+    Matrix<float, 6, 6> tmp = Matrix<float, 6, 6>::Identity() - K * H;
+    Pq = tmp * Pq * tmp.transpose() + K * R_mag * K.transpose();
+    P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
+}
+
+void NAS::correctAcc(const Eigen::Vector3f& acc)
+{
+    Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
+    Matrix3f A = body2ned(q).transpose();
+
+    // Rotate the NED magnetic field in the relative reference frame
+    Vector3f aEst = A * gravityNed;
+
+    // Gradient matrix
+    // clang-format off
+    Matrix3f M{
+        { 0,      -aEst(2), aEst(1)},
+        { aEst(2), 0,      -aEst(0)},
+        {-aEst(1), aEst(0), 0}
+    };
+    // clang-format on
+
+    Matrix<float, 3, 6> H;
+    H << M, Matrix3f::Zero(3, 3);
+    Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT);
+    Matrix<float, 3, 3> S =
+        H * Pq * H.transpose() + R_mag;  // TODO: Change R_mag with R_acc
+
+    Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse();
+
+    aEst.normalize();
+    Vector3f e             = acc - aEst;
+    Matrix<float, 6, 1> dx = K * e;
+    Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2),
+               sqrtf(1.0f - 0.25f * dx.head<3>().squaredNorm())};
+
+    x.block<4, 1>(IDX_QUAT, 0) = SkyQuaternion::quatProd(r, q);
+    x.block<3, 1>(IDX_BIAS, 0) += dx.tail<3>();
+    Matrix<float, 6, 6> tmp = Matrix<float, 6, 6>::Identity() - K * H;
+    Pq = tmp * Pq * tmp.transpose() + K * R_mag * K.transpose();
+    P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
+}
+
+void NAS::correctPitot(const float deltaP, const float staticP)
+{
+    if (deltaP >= 0)
+    {
+        float c      = 343;  // Speed of sound
+        Vector3f vel = x.block<3, 1>(IDX_VEL, 0);
+        float vPitot;
+
+        if (vel.norm() / c >= 0.9419)
+        {
+            float rho = 1.225;  // Air density
+            vPitot    = -sqrtf(
+                   fabs(-2.0f * c * c +
+                        sqrtf((4.0f * powf(c, 4) + 8.0f * c * c * deltaP / rho))));
+        }
+        else
+        {
+            float gamma = 1.4;  // Heat capacity ratio of dry air
+            float p0    = staticP + deltaP;
+            vPitot =
+                -sqrtf(c * c * 2 / (gamma - 1) *
+                       fabs(powf((p0 / staticP), (gamma - 1) / gamma) - 1));
+        }
+
+        Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
+        H.coeffRef(0, 5)      = 1;
+
+        Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+        Matrix<float, 1, 1> S =
+            H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_PITOT);
+
+        Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse();
+        x.head<6>()           = x.head<6>() + K * (vPitot - x[5]);
+        P.block<6, 6>(0, 0)   = (Matrix<float, 6, 6>::Identity() - K * H) * Pl;
+    }
+}
+
+NASState NAS::getState() const
+{
+    return NASState(TimestampTimer::getInstance().getTimestamp(), x);
+}
+
+Eigen::Matrix<float, 13, 1> NAS::getX() const { return x; }
+
+void NAS::setState(const NASState& state) { this->x = state.getX(); }
+
+void NAS::setX(const Eigen::Matrix<float, 13, 1>& x) { this->x = x; }
+
+Matrix3f NAS::body2ned(const Vector4f& q)
+{
+    // clang-format off
+    return Matrix3f{
+        {
+            q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3],
+            2.0f * (q[0] * q[1] - q[2] * q[3]),
+            2.0f * (q[0] * q[2] + q[1] * q[3]),
+        },
+        {
+            2.0f * (q[0] * q[1] + q[2] * q[3]),
+            - q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3],
+            2.0f * (q[1] * q[2] - q[0] * q[3]),
+        },
+        {
+            2.0f * (q[0] * q[2] - q[1] * q[3]),
+            2.0f * (q[1] * q[2] + q[0] * q[3]),
+            - q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3]
+        }
+    };
+    // clang-format on
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/NAS/NAS.h b/src/shared/algorithms/NAS/NAS.h
new file mode 100644
index 0000000000000000000000000000000000000000..d232f4beb6f6951c1d8ecded27b3aea6c2c9a77f
--- /dev/null
+++ b/src/shared/algorithms/NAS/NAS.h
@@ -0,0 +1,168 @@
+/* Copyright (c) 2020-2022 Skyward Experimental Rocketry
+ * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella
+ *
+ * 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 <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+
+#include <Eigen/Dense>
+
+#include "NASConfig.h"
+#include "NASState.h"
+
+namespace Boardcore
+{
+
+class NAS
+{
+public:
+    ///< Index of position elements in the state.
+    static constexpr uint16_t IDX_POS = 0;
+
+    ///< Index of velocity elements in the state.
+    static constexpr uint16_t IDX_VEL = 3;
+
+    ///< Index of quaternions elements in the state.
+    static constexpr uint16_t IDX_QUAT = 6;
+
+    ///< Index of bias elements in the state.
+    static constexpr uint16_t IDX_BIAS = 10;
+
+    explicit NAS(NASConfig config);
+
+    /**
+     * @brief Prediction with accelerometer data.
+     *
+     * @param u Vector with acceleration data [x y z][m/s^2]
+     */
+    void predictAcc(const Eigen::Vector3f& acceleration);
+
+    /**
+     * @brief Prediction with gyroscope data.
+     *
+     * @param u Vector with angular velocity data [x y z]
+     */
+    void predictGyro(const Eigen::Vector3f& angularVelocity);
+
+    /**
+     * @brief Correction with barometer data.
+     *
+     * @param pressure Pressure read from the barometer [Pa]
+     * @param mslPress Pressure at sea level [Pa]
+     * @param mslTemp Temperature at sea level [Kelvin]
+     */
+    void correctBaro(const float pressure, const float mslPress,
+                     const float mslTemp);
+
+    /**
+     * @brief Correction with gps data.
+     *
+     * @param gps Vector of the gps readings [n e vn ve][m m m/s m/s]
+     * @param sats_num Number of satellites available
+     */
+    void correctGPS(const Eigen::Vector4f& gps);
+
+    /**
+     * @brief Correction with magnetometer data.
+     *
+     * @param mag Normalized vector of the magnetometer readings [x y z]
+     */
+    void correctMag(const Eigen::Vector3f& mag);
+
+    /**
+     * @brief Correction with accelerometer data.
+     *
+     * @param u Normaliezed vector with acceleration data [x y z]
+     */
+    void correctAcc(const Eigen::Vector3f& acceleration);
+
+    /**
+     * @brief Correction with pitot pressure.
+     *
+     * Do not call this function after apogee!
+     *
+     * @param deltaP Delta pressure measured by the differential sensor [Pa]
+     * @param staticP Static pressure [Pa]
+     */
+    void correctPitot(const float deltaP, const float staticP);
+
+    /**
+     * @return EKF state.
+     */
+    NASState getState() const;
+
+    /**
+     * @return State vector [n e d vn ve vd qx qy qz qw bx by bz].
+     */
+    Eigen::Matrix<float, 13, 1> getX() const;
+
+    /**
+     * @param state EKF state.
+     */
+    void setState(const NASState& state);
+
+    /**
+     * @param state State vector [n e d vn ve vd qx qy qz qw bx by bz].
+     */
+    void setX(const Eigen::Matrix<float, 13, 1>& x);
+
+private:
+    /**
+     * @brief Return a rotation matrix from body from to NED frame;
+     *
+     * TODO: Move to SkyQuaternion utilities
+     */
+    Eigen::Matrix3f body2ned(const Eigen::Vector4f& q);
+
+    ///< Extended Kalman filter configuration parameters
+    NASConfig config;
+
+    ///< State vector [n e d vn ve vd qx qy qz qw bx by bz]
+    Eigen::Matrix<float, 13, 1> x;
+
+    ///< Covariance matrix
+    Eigen::Matrix<float, 12, 12> P;
+
+    ///< NED gravity vector [m/s^2]
+    Eigen::Vector3f gravityNed{0.0f, 0.0f, -Constants::g};
+
+    // Utility matrixes used for the gps
+    Eigen::Matrix<float, 4, 6> H_gps;
+    Eigen::Matrix<float, 6, 4> H_gps_tr;
+    Eigen::Matrix<float, 4, 4> R_gps;
+
+    // Utility matrixes used for the magnetometer
+    Eigen::Matrix3f R_mag;
+
+    // Other utility matrixes
+    Eigen::Matrix<float, 6, 6> Q_quat;
+    Eigen::Matrix<float, 6, 6> Q_lin;
+    Eigen::Matrix<float, 6, 6> F_lin;
+    Eigen::Matrix<float, 6, 6> F_lin_tr;
+    Eigen::Matrix<float, 6, 6> F_quat;
+    Eigen::Matrix<float, 6, 6> F_quat_tr;
+    Eigen::Matrix<float, 6, 6> G_quat;
+    Eigen::Matrix<float, 6, 6> G_quat_tr;
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/NAS/NASConfig.h b/src/shared/algorithms/NAS/NASConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..b73b2f8f8b3bdf6a5eb1d48194d81f9a8ad0ad75
--- /dev/null
+++ b/src/shared/algorithms/NAS/NASConfig.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace Boardcore
+{
+
+struct NASConfig
+{
+    float T;            ///< [s]       Sample period
+    float SIGMA_BETA;   ///< [rad/s^2] Estimated gyroscope bias variance
+    float SIGMA_W;      ///< [rad^2]   Estimated gyroscope variance
+    float SIGMA_MAG;    ///< [uT^2]    Estimated magnetometer variance
+    float SIGMA_GPS;    ///< [m^2]     Estimated GPS variance
+    float SIGMA_BAR;    ///< [m^2]     Estimated altitude variance
+    float SIGMA_POS;    ///< [m^2]     Estimated variance of the position noise
+    float SIGMA_VEL;    ///< [(m/s)^2] Estimated variance of the velocity noise
+    float SIGMA_PITOT;  ///< [Pa^2]    Estimated variance of the pitot velocity
+
+    float P_POS;  ///< Position prediction covariance
+    float P_POS_VERTICAL;
+
+    float P_VEL;  ///< Velocity prediction covariance
+    float P_VEL_VERTICAL;
+
+    float P_ATT;   ///< Attitude prediction covariance
+    float P_BIAS;  ///< Bias prediction covariance
+
+    float SATS_NUM = 6.0f;  ///< Number of satellites used at setup time
+
+    Eigen::Vector3f NED_MAG;  ///< Normalized magnetic field vector in NED frame
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/NAS/NASState.h b/src/shared/algorithms/NAS/NASState.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c5a39db9c12ba68a643b5755a1bd26085ebd205
--- /dev/null
+++ b/src/shared/algorithms/NAS/NASState.h
@@ -0,0 +1,85 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+namespace Boardcore
+{
+
+struct NASState
+{
+    uint64_t timestamp;
+
+    // 13 extended kalman states
+
+    // Position [m]
+    float n = 0;
+    float e = 0;
+    float d = 0;
+
+    // Velocity [m/s]
+    float vn = 0;
+    float ve = 0;
+    float vd = 0;
+
+    // Attitude as quaternion
+    float qx = 0;
+    float qy = 0;
+    float qz = 0;
+    float qw = 1;
+
+    // Gyroscope bias
+    float bx = 0;
+    float by = 0;
+    float bz = 0;
+
+    NASState() {}
+
+    NASState(uint64_t timestamp, Eigen::Matrix<float, 13, 1> x)
+        : timestamp(timestamp), n(x(0)), e(x(1)), d(x(2)), vn(x(3)), ve(x(4)),
+          vd(x(5)), qx(x(6)), qy(x(7)), qz(x(8)), qw(x(9)), bx(x(10)),
+          by(x(11)), bz(x(12))
+    {
+    }
+
+    Eigen::Matrix<float, 13, 1> getX() const
+    {
+        return Eigen::Matrix<float, 13, 1>(n, e, d, vn, ve, vd, qx, qy, qz, qw,
+                                           bx, by, bz);
+    }
+
+    static std::string header()
+    {
+        return "timestamp,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << n << "," << e << "," << d << "," << vn << ","
+           << ve << "," << vd << "," << qx << "," << qy << "," << qz << ","
+           << qw << "," << bx << "," << by << "," << bz << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/NAS/StateInitializer.h b/src/shared/algorithms/NAS/StateInitializer.h
new file mode 100644
index 0000000000000000000000000000000000000000..959597beb70db8e4b2133eacf12df22d90f10add
--- /dev/null
+++ b/src/shared/algorithms/NAS/StateInitializer.h
@@ -0,0 +1,169 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Marco Cella, 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 <diagnostic/PrintLogger.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <Eigen/Dense>
+
+#include "NAS.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Utility used to initialize the extended kalman filter's state.
+ *
+ * The intended use is the following:
+ * - Instantiate the object, the state is initialize to zero;
+ * - Call positionInit to set the initial position;
+ * - Call either eCompass or triad to set the initial orientation;
+ * - Get the initial state for the kalman with getInitX
+ */
+class StateInitializer
+{
+public:
+    /**
+     * @brief Initialize the state of the Extended Kalman Filter.
+     *
+     * The state is initialized to zero. Velocity should be null and gyro biases
+     * can be left to zero since the sensor performs self-calibration and the
+     * measurements are already compensated.
+     */
+    StateInitializer();
+
+    /**
+     * @brief Ecompass algorithm to estimate the attitude before the liftoff.
+     *
+     * ecompass reference:
+     *   https://de.mathworks.com/help/fusion/ref/ecompass.html
+     *
+     * @param acc 3x1 accelerometer readings [x y z][m/s^2].
+     * @param mag 3x1 magnetometer readings [x y z][uT].
+     *
+     */
+    void eCompass(const Eigen::Vector3f acc, const Eigen::Vector3f mag);
+
+    /**
+     * @brief Triad algorithm to estimate the attitude before the liftoff.
+     *
+     * triad reference:
+     *   https://en.wikipedia.org/wiki/Triad_method
+     *   https://www.aero.iitb.ac.in/satelliteWiki/index.php/Triad_Algorithm
+     *
+     * @param acc Normalized accelerometer readings [x y z].
+     * @param mag Normalized magnetometer readings [x y z].
+     * @param nedMag Normalized magnetic field vector in NED frame [x y z].
+     */
+    void triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
+               Eigen::Vector3f& nedMag);
+
+    /**
+     * @brief Initialization of the position at a specific altitude
+     *
+     * @param pressure Current pressure [Pas]
+     * @param pressureRef Pressure at reference altitude (must be > 0) [Pa]
+     * @param temperatureRef Temperature at reference altitude [K]
+     */
+    void positionInit(const float pressure, const float pressureRef,
+                      const float temperatureRef);
+
+    Eigen::Matrix<float, 13, 1> getInitX();
+
+private:
+    Eigen::Matrix<float, 13, 1> x_init;
+
+    PrintLogger log = Logging::getLogger("initstates");
+};
+
+StateInitializer::StateInitializer() { x_init << Eigen::MatrixXf::Zero(13, 1); }
+
+void StateInitializer::eCompass(const Eigen::Vector3f acc,
+                                const Eigen::Vector3f mag)
+{
+    // ndr: since this method runs only when the rocket is stationary, there's
+    // no need to add the gravity vector because the accelerometers already
+    // measure it. This is not true if we consider the flying rocket.
+
+    Eigen::Vector3f am(acc.cross(mag));
+
+    Eigen::Matrix3f R;
+    R << am.cross(acc), am, acc;
+    R.col(0).normalize();
+    R.col(1).normalize();
+    R.col(2).normalize();
+
+    Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
+
+    x_init(NAS::IDX_QUAT)     = x_quat(0);
+    x_init(NAS::IDX_QUAT + 1) = x_quat(1);
+    x_init(NAS::IDX_QUAT + 2) = x_quat(2);
+    x_init(NAS::IDX_QUAT + 3) = x_quat(3);
+}
+
+void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
+                             Eigen::Vector3f& nedMag)
+{
+    // The gravity vector is expected to be read inversely because
+    // accelerometers read the binding reaction
+    Eigen::Vector3f nedAcc(0.0f, 0.0f, -1.0f);
+
+    // Compute the reference triad
+    Eigen::Vector3f R1 = nedAcc;
+    Eigen::Vector3f R2 = nedAcc.cross(nedMag).normalized();
+    Eigen::Vector3f R3 = R1.cross(R2);
+
+    // Compute the measured triad
+    Eigen::Vector3f r1 = acc;
+    Eigen::Vector3f r2 = acc.cross(mag).normalized();
+    Eigen::Vector3f r3 = r1.cross(r2);
+
+    // Compose the reference and measured matrixes
+    Eigen::Matrix3f M;
+    M << R1, R2, R3;
+    Eigen::Matrix3f m;
+    m << r1, r2, r3;
+
+    // Compute the rotation matrix and the corresponding quaternion
+    Eigen::Matrix3f A = M * m.transpose();
+    Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
+
+    // Save the orientation in the state
+    x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q;
+}
+
+void StateInitializer::positionInit(const float pressure,
+                                    const float pressureRef,
+                                    const float temperatureRef)
+{
+    x_init(0) = 0.0;
+    x_init(1) = 0.0;
+    // msl altitude (in NED, so negative)
+    x_init(2) = -Aeroutils::relAltitude(pressure, pressureRef, temperatureRef);
+}
+
+Eigen::Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; }
+
+}  // namespace Boardcore
diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp b/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp
deleted file mode 100644
index c3fe8804bd3368b1171befeaf24d688df91f7c39..0000000000000000000000000000000000000000
--- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp
+++ /dev/null
@@ -1,304 +0,0 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella
- *
- * 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 <NavigationAttitudeSystem/ExtendedKalmanEigen.h>
-
-using namespace Boardcore;
-using namespace Eigen;
-
-namespace DeathStackBoard
-{
-
-using namespace NASConfigs;
-
-ExtendedKalmanEigen::ExtendedKalmanEigen()
-{
-    eye6.setIdentity();
-    eye4.setIdentity();
-    eye3.setIdentity();
-    eye2.setIdentity();
-
-    // clang-format off
-
-    R_bar << SIGMA_BAR;
-    R_mag << SIGMA_MAG * eye3;
-    R_gps << eye4 * SIGMA_GPS / SATS_NUM;
-    Q_mag << (SIGMA_W * SIGMA_W * T +
-              (1.0F / 3.0F) * SIGMA_BETA * SIGMA_BETA * T * T * T) *
-                 eye3,
-        (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3,
-        // cppcheck-suppress constStatement
-        (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3,
-        (SIGMA_BETA * SIGMA_BETA * T) * eye3;
-
-    P_pos << P_POS, 0,     0,
-             0,     P_POS, 0,
-             0,     0,     P_POS_VERTICAL;
-    P_vel << P_VEL, 0,     0,
-             0,     P_VEL, 0,
-             0,     0,     P_VEL_VERTICAL;
-    P_att  = eye3 * P_ATT;
-    P_bias = eye3 * P_BIAS;
-    P << P_pos, Eigen::MatrixXf::Zero(3, N - 4), Eigen::MatrixXf::Zero(3, 3), P_vel,
-        Eigen::MatrixXf::Zero(3, N - 7), Eigen::MatrixXf::Zero(3, 6), P_att,
-        Eigen::MatrixXf::Zero(3, N - 10), Eigen::MatrixXf::Zero(3, 9), P_bias;
-
-    Q_pos = eye3 * SIGMA_POS;
-    Q_vel = eye3 * SIGMA_VEL;
-    Q_lin << Q_pos, Eigen::MatrixXf::Zero(3, NL - 3), Eigen::MatrixXf::Zero(3, 3), Q_vel;
-
-    F << 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F,
-         0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F,
-         0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F,
-         Eigen::MatrixXf::Zero(3, NL);
-    F   = eye6 + T * F;
-    Ftr = F.transpose();
-
-    H_gps << 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-             0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-             0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F,
-             // cppcheck-suppress constStatement
-             0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F;
-    H_gpstr = H_gps.transpose();
-
-    Fatt << -eye3, -eye3 * T, Eigen::Matrix3f::Zero(3, 3), eye3;
-    Fatttr = Fatt.transpose();
-
-    Gatt << -eye3, Eigen::Matrix3f::Zero(3, 3),
-            Eigen::Matrix3f::Zero(3, 3), eye3;
-    Gatttr = Gatt.transpose();
-
-    g << 0.0F, 0.0F, aeroutils::constants::g; // [m/s^2]
-
-    // clang-format on
-}
-
-void ExtendedKalmanEigen::predict(const Eigen::Vector3f& u)
-{
-    Eigen::Matrix3f A;
-    Eigen::Vector3f a;
-    VectorNf x_dot;
-
-    Plin                  = P.block<NL, NL>(0, 0);
-    P.block<NL, NL>(0, 0) = F * Plin * Ftr + Q_lin;
-
-    float q1 =
-        x(NL);  // Since the index begins from 0, the first quaternion
-                // component will be at index  = # linear states before itself
-    float q2 = x(NL + 1);
-    float q3 = x(NL + 2);
-    float q4 = x(NL + 3);
-
-    A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2),
-        2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4,
-        2.0F * q1 * q2 - 2.0F * q3 * q4,
-        -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2),
-        2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4,
-        // cppcheck-suppress constStatement
-        2.0F * q2 * q3 - 2.0F * q1 * q4,
-        -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2);
-
-    a = A.transpose() * u +
-        g;  // Real accelerometers don't measure g during the flight
-
-    x_dot << x(3), x(4), x(5), a,
-        Eigen::MatrixXf::Zero(
-            NATT,
-            1);  // The quaternions and the biases don't need to be updated
-
-    x = x + T * x_dot;
-}
-
-void ExtendedKalmanEigen::correctBaro(const float y, const float msl_press,
-                                      const float msl_temp)
-{
-    Matrix<float, NL, NBAR> K_bar;
-    Matrix<float, NBAR, NL> H_bar;
-    Matrix<float, NBAR, NBAR> S_bar;
-
-    Plin = P.block<NL, NL>(0, 0);
-
-    // temperature at current altitude :
-    // since x(2) (altitude) is negative, mslTemperature returns temperature at
-    // current altitude and not at mean sea level
-    float temp = aeroutils::mslTemperature(msl_temp, x(2));
-
-    // compute gradient of the altitude-pressure function
-    float dp_dx = aeroutils::constants::a * aeroutils::constants::n *
-                  msl_press *
-                  powf(1 - aeroutils::constants::a * x(2) / temp,
-                       -aeroutils::constants::n - 1) /
-                  temp;
-
-    H_bar << 0.0F, 0.0F, dp_dx,
-        Eigen::MatrixXf::Zero(1, N - 3 - NATT);  // Gradient of h_bar
-
-    S_bar = H_bar * Plin * H_bar.transpose() + R_bar;
-
-    K_bar = Plin * H_bar.transpose() * S_bar.inverse();
-
-    P.block<NL, NL>(0, 0) = (eye6 - K_bar * H_bar) * Plin;
-
-    float y_hat = aeroutils::mslPressure(msl_press, msl_temp, x(2));
-
-    x.head(NL) = x.head(NL) + K_bar * (y - y_hat);
-
-    // float res_bar = y - h_bar;
-}
-
-void ExtendedKalmanEigen::correctGPS(const Vector4f& y, const uint8_t sats_num)
-{
-    Matrix<float, NGPS, 1> h_gps;
-    Matrix<float, NL, NGPS> K_gps;
-    Matrix<float, NGPS, 1> res_gps;
-    Matrix<float, NGPS, NGPS> S_gps;
-
-    float xnord   = y(0);
-    float yest    = y(1);
-    float velnord = y(2);
-    float velest  = y(3);
-
-    Matrix<float, NGPS, 1> yned(xnord, yest, velnord, velest);
-
-    R_gps = eye4 * SIGMA_GPS / sqrtf(sats_num);
-
-    Plin = P.block<NL, NL>(0, 0);
-
-    S_gps = H_gps * Plin * H_gpstr + R_gps;
-
-    K_gps = Plin * H_gpstr * S_gps.inverse();
-
-    P.block<NL, NL>(0, 0) = (eye6 - K_gps * H_gps) * Plin;
-
-    h_gps << x(0), x(1), x(3), x(4);
-
-    x.head(NL) = x.head(NL) + K_gps * (yned - h_gps);
-
-    // cppcheck-suppress unreadVariable
-    res_gps = y - h_gps;
-}
-
-const VectorNf& ExtendedKalmanEigen::getState() { return x; }
-
-void ExtendedKalmanEigen::setX(const VectorNf& x) { this->x = x; }
-
-/*
-    MULTIPLICATIVE EXTENDED KALMAN FILTER
-*/
-void ExtendedKalmanEigen::predictMEKF(const Eigen::Vector3f& u)
-{
-    Eigen::Vector3f omega;
-    Eigen::Vector3f prev_bias;
-    Matrix4f omega_mat;
-    Eigen::Matrix3f omega_submat;
-
-    q << x(NL), x(NL + 1), x(NL + 2), x(NL + 3);
-    prev_bias << x(NL + 4), x(NL + 5), x(NL + 6);
-
-    omega = u - prev_bias;
-
-    omega_submat << 0.0F, -omega(2), omega(1), omega(2), 0.0F, -omega(0),
-        // cppcheck-suppress constStatement
-        -omega(1), omega(0), 0.0F;
-
-    // cppcheck-suppress constStatement
-    omega_mat << -omega_submat, omega, -omega.transpose(), 0.0F;
-
-    q = (eye4 + 0.5F * omega_mat * T) * q;
-    q.normalize();
-
-    // cppcheck-suppress constStatement
-    x.tail(NATT) << q, prev_bias;
-
-    Patt = P.block<NMEKF, NMEKF>(
-        NL, NL);  // The block of P related to the attitude starts at (NL, NL)
-                  // because the index starts from 0. The block is of size
-                  // NMEKF x NMEKF.
-    P.block<NMEKF, NMEKF>(NL, NL) =
-        Fatt * Patt * Fatttr +
-        Gatt * Q_mag * Gatttr;  // Update only the attitude related part of P
-}
-
-void ExtendedKalmanEigen::correctMEKF(const Eigen::Vector3f& y)
-{
-    Eigen::Matrix3f A;
-    Eigen::Vector3f z;
-    Vector4f r;
-    Eigen::Matrix3f z_mat;
-    Vector4f u_att;
-    Eigen::Vector3f r_sub;
-    Matrix<float, NMAG, NMAG> Satt;
-    Matrix<float, NMAG, NMEKF> Hatt;
-    Matrix<float, NMEKF, NMAG> Katt;
-    Matrix<float, NMEKF, 1> delta_x;
-
-    float q1 = x(NL);
-    float q2 = x(NL + 1);
-    float q3 = x(NL + 2);
-    float q4 = x(NL + 3);
-
-    A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2),
-        2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4,
-        2.0F * q1 * q2 - 2.0F * q3 * q4,
-        -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2),
-        2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4,
-        // cppcheck-suppress constStatement
-        2.0F * q2 * q3 - 2.0F * q1 * q4,
-        -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2);
-
-    z = A * NED_MAG;
-
-    // cppcheck-suppress constStatement
-    z_mat << 0.0F, -z(2), z(1), z(2), 0.0F, -z(0), -z(1), z(0), 0.0F;
-
-    Hatt << z_mat, Eigen::Matrix3f::Zero(3, 3);
-
-    Patt = P.block<NMEKF, NMEKF>(NL, NL);
-    Satt = Hatt * Patt * Hatt.transpose() + R_mag;
-
-    Katt = Patt * Hatt.transpose() * Satt.inverse();
-
-    delta_x = Katt * (y - z);
-
-    r_sub << delta_x(0), delta_x(1), delta_x(2);
-
-    r << 0.5F * r_sub, sqrtf(1.0F - 0.25F * r_sub.transpose() * r_sub);
-
-    // cppcheck-suppress constStatement
-    q << q1, q2, q3, q4;
-    u_att        = quater.quatProd(r, q);
-    float u_norm = u_att.norm();
-
-    x(NL)     = u_att(0) / u_norm;
-    x(NL + 1) = u_att(1) / u_norm;
-    x(NL + 2) = u_att(2) / u_norm;
-    x(NL + 3) = u_att(3) / u_norm;
-    x(NL + 4) = x(NL + 4) + delta_x(3);
-    x(NL + 5) = x(NL + 5) + delta_x(4);
-    x(NL + 6) = x(NL + 6) + delta_x(5);
-
-    P.block<NMEKF, NMEKF>(NL, NL) =
-        (eye6 - Katt * Hatt) * Patt * (eye6 - Katt * Hatt).transpose() +
-        Katt * R_mag * Katt.transpose();
-}
-
-}  // namespace DeathStackBoard
diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h b/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h
deleted file mode 100644
index 4b1f5ca73bbd29ddbae0a5f310eda04da63c8aba..0000000000000000000000000000000000000000
--- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella
- *
- * 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 <configs/NASConfig.h>
-#include <math/SkyQuaternion.h>
-#include <utils/aero/AeroUtils.h>
-
-#include <Eigen/Dense>
-
-namespace DeathStackBoard
-{
-
-using VectorNf = Eigen::Matrix<float, NASConfigs::N, 1>;
-
-class ExtendedKalmanEigen
-{
-
-public:
-    ExtendedKalmanEigen();
-
-    /**
-     * @brief Prediction step of the EKF.
-     *
-     * @param u 3x1 Vector of the accelerometer readings [ax ay az].
-     */
-    void predict(const Eigen::Vector3f& u);
-
-    /**
-     * @brief EKF correction of the barometer data.
-     *
-     * @param y Pressure read from the barometer [Pa]
-     * @param msl_press Pressure at sea level [Pa]
-     * @param msl_temp Temperature at sea level [Kelvin]
-     */
-    void correctBaro(const float y, const float msl_press,
-                     const float msl_temp);
-
-    /**
-     * @brief EKF correction of the gps readings.
-     *
-     * @param y 4x1 Vector of the gps readings [longitude, latitude,
-     * gps_nord_vel, gps_east_vel].
-     * @param sats_num Number of satellites available
-     */
-    void correctGPS(const Eigen::Vector4f& y, const uint8_t sats_num);
-
-    /**
-     * @brief Prediction step of the Multiplicative EKF.
-     *
-     * @param u 3x1 Vector of the gyroscope readings [wx wy wz].
-     */
-    void predictMEKF(const Eigen::Vector3f& u);
-
-    /**
-     * @brief MEKF correction of the magnetometer readings.
-     *
-     * @param y 3x1 Vector of the magnetometer readings [mx my mz].
-     */
-    void correctMEKF(const Eigen::Vector3f& y);
-
-    /**
-     * @return 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz].
-     */
-    const VectorNf& getState();
-
-    /**
-     * @param x 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz].
-     */
-    void setX(const VectorNf& x);
-
-private:
-    VectorNf x;
-    Eigen::Matrix<float, NASConfigs::NP, NASConfigs::NP> P;
-    Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> F;
-    Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Ftr;
-
-    Eigen::Matrix3f P_pos;
-    Eigen::Matrix3f P_vel;
-    Eigen::Matrix3f P_att;
-    Eigen::Matrix3f P_bias;
-    Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Plin;
-
-    Eigen::Matrix3f Q_pos;
-    Eigen::Matrix3f Q_vel;
-    Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Q_lin;
-
-    Eigen::Vector3f g;
-    Eigen::Matrix2f eye2;
-    Eigen::Matrix3f eye3;
-    Eigen::Matrix4f eye4;
-    Eigen::Matrix<float, 6, 6> eye6;
-
-    Eigen::Matrix<float, NASConfigs::NBAR, NASConfigs::NBAR> R_bar;
-
-    Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NGPS> R_gps;
-    Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NL> H_gps;
-    Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NGPS> H_gpstr;
-
-    Eigen::Vector4f q;
-    Eigen::Matrix<float, NASConfigs::NMAG, NASConfigs::NMAG> R_mag;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Q_mag;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatt;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatttr;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatt;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatttr;
-    Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Patt;
-
-    Boardcore::SkyQuaternion quater;
-};
-
-}  // namespace DeathStackBoard
diff --git a/src/shared/diagnostic/LogSink.h b/src/shared/diagnostic/LogSink.h
index 1efbaf52e445a8e8616f0c213688a85e048c3db2..c06917f36c5edf7869f92b7240815ff1a89322a9 100644
--- a/src/shared/diagnostic/LogSink.h
+++ b/src/shared/diagnostic/LogSink.h
@@ -33,7 +33,7 @@ class LogSink
 {
 public:
     LogSink() {}
-    LogSink(const LogSink&) = delete;
+    LogSink(const LogSink&)            = delete;
     LogSink& operator=(const LogSink&) = delete;
 
     virtual ~LogSink() {}
diff --git a/src/shared/diagnostic/PrintLogger.h b/src/shared/diagnostic/PrintLogger.h
index 02758ee784a9e63a93d285283741efa2d5b8dd23..d8b253b77e4aa9264b0227fd32926af43ef1ee36 100644
--- a/src/shared/diagnostic/PrintLogger.h
+++ b/src/shared/diagnostic/PrintLogger.h
@@ -100,7 +100,7 @@ class Logging : public Singleton<Logging>
     friend class PrintLogger;
 
 public:
-    static PrintLogger getLogger(string name)
+    static PrintLogger getLogger(const string& name)
     {
         return PrintLogger(getInstance(), name);
     }
diff --git a/src/shared/diagnostic/StackData.h b/src/shared/diagnostic/StackData.h
index 5e9f58b702cc7d3490832fc71880131875748707..4441aafd0c3891986e11d5242f58207f2b961698 100644
--- a/src/shared/diagnostic/StackData.h
+++ b/src/shared/diagnostic/StackData.h
@@ -35,6 +35,7 @@ enum ThreadId : uint8_t
     THID_MAV_RECEIVER,
     THID_MAV_SENDER,
     THID_XBEE,
+    THID_GPS,
     THID_EVT_BROKER,
     THID_LOGGER_PACK,
     THID_LOGGER_WRITE,
diff --git a/src/shared/drivers/runcam/Runcam.h b/src/shared/drivers/runcam/Runcam.h
index fe089afee56c02845d0f084be8ec3ceb7bbaea00..eabb564aee0b43d768e2b0c15fa0f490f4d2310f 100644
--- a/src/shared/drivers/runcam/Runcam.h
+++ b/src/shared/drivers/runcam/Runcam.h
@@ -130,7 +130,7 @@ public:
 
 private:
     /**
-     * @brief Confiugre Serial Communication
+     * @brief Configure Serial Communication
      */
     bool configureSerialCommunication();
 
diff --git a/src/shared/drivers/spi/SPI.h b/src/shared/drivers/spi/SPI.h
index 855cd260a39ab06d43984776306e97127f953f51..950e276edc563aae76dc714ee95aa078457fe1a2 100644
--- a/src/shared/drivers/spi/SPI.h
+++ b/src/shared/drivers/spi/SPI.h
@@ -29,7 +29,7 @@
 #ifndef USE_MOCK_PERIPHERALS
 using SPIType = SPI_TypeDef;
 #else
-#include <utils/testutils/FakeSpiTypedef.h>
+#include <utils/TestUtils/FakeSpiTypedef.h>
 using SPIType = FakeSpiTypedef;
 #endif
 
@@ -205,7 +205,7 @@ public:
      * @param data Buffer containing data to write.
      * @param size Size of the buffer in bytes.
      */
-    void write(uint8_t *data, size_t nBytes);
+    void write(const uint8_t *data, size_t nBytes);
 
     /**
      * @brief Writes multiple half words to the bus.
@@ -213,7 +213,7 @@ public:
      * @param data Buffer containing data to write.
      * @param size Size of the buffer in bytes.
      */
-    void write(uint16_t *data, size_t nBytes);
+    void write(const uint16_t *data, size_t nBytes);
 
     /**
      * @brief Full duplex transmission of one byte on the bus.
@@ -393,7 +393,7 @@ inline void SPI::write(uint16_t data)
     (void)spi->DR;
 }
 
-inline void SPI::write(uint8_t *data, size_t nBytes)
+inline void SPI::write(const uint8_t *data, size_t nBytes)
 {
     // Write the first data item in the Tx buffer
     spi->DR = data[0];
@@ -419,7 +419,7 @@ inline void SPI::write(uint8_t *data, size_t nBytes)
     (void)spi->DR;
 }
 
-inline void SPI::write(uint16_t *data, size_t nBytes)
+inline void SPI::write(const uint16_t *data, size_t nBytes)
 {
     // Set 16 bit frame format
     set16BitFrameFormat();
diff --git a/src/shared/drivers/spi/SPIBus.h b/src/shared/drivers/spi/SPIBus.h
index 4706f158dcf519ee68ba74ae1830de7441b78e03..0d5b0c0b7faeb8c166fd55b8e454a1409ba22aac 100644
--- a/src/shared/drivers/spi/SPIBus.h
+++ b/src/shared/drivers/spi/SPIBus.h
@@ -46,10 +46,10 @@ public:
     SPIBus(SPIType* spi);
 
     ///< Delete copy/move contructors/operators.
-    SPIBus(const SPIBus&) = delete;
+    SPIBus(const SPIBus&)            = delete;
     SPIBus& operator=(const SPIBus&) = delete;
     SPIBus(SPIBus&&)                 = delete;
-    SPIBus& operator=(SPIBus&&) = delete;
+    SPIBus& operator=(SPIBus&&)      = delete;
 
     /**
      * @brief Configures and enables the bus with the provided configuration.
@@ -168,37 +168,43 @@ public:
 private:
     SPI spi;
     SPIBusConfig config{};
+    bool firstConfigApplied = false;
 };
 
 inline SPIBus::SPIBus(SPIType* spi) : spi(spi) {}
 
 inline void SPIBus::configure(SPIBusConfig newConfig)
 {
-    // Save the new configuration
-    config = newConfig;
+    // Do not reconfigure if already in the correct configuration.
+    if (!firstConfigApplied || newConfig != config)
+    {
+        // Save the new configuration
+        config             = newConfig;
+        firstConfigApplied = true;
 
-    // Wait until the peripheral is done before changing configuration
-    spi.waitPeripheral();
+        // Wait until the peripheral is done before changing configuration
+        spi.waitPeripheral();
 
-    // Disable the peripheral
-    spi.disable();
+        // Disable the peripheral
+        spi.disable();
 
-    // Configure clock polarity and phase
-    spi.setMode(config.mode);
+        // Configure clock polarity and phase
+        spi.setMode(config.mode);
 
-    // Configure clock frequency
-    spi.setClockDiver(config.clockDivider);
+        // Configure clock frequency
+        spi.setClockDiver(config.clockDivider);
 
-    // Configure bit order
-    spi.setBitOrder(config.bitOrder);
+        // Configure bit order
+        spi.setBitOrder(config.bitOrder);
 
-    // Configure chip select and master mode
-    spi.enableSoftwareSlaveManagement();
-    spi.enableInternalSlaveSelection();
-    spi.setMasterConfiguration();
+        // Configure chip select and master mode
+        spi.enableSoftwareSlaveManagement();
+        spi.enableInternalSlaveSelection();
+        spi.setMasterConfiguration();
 
-    // Enable the peripheral
-    spi.enable();
+        // Enable the peripheral
+        spi.enable();
+    }
 }
 
 inline void SPIBus::select(GpioType& cs)
diff --git a/src/shared/drivers/spi/SPIBusInterface.h b/src/shared/drivers/spi/SPIBusInterface.h
index e0eb8bd38f5366451aff51f82aec89102fbb328e..f10a7e33228d5b43bf701e5e8178cead2abf8411 100644
--- a/src/shared/drivers/spi/SPIBusInterface.h
+++ b/src/shared/drivers/spi/SPIBusInterface.h
@@ -29,7 +29,7 @@
 #ifndef USE_MOCK_PERIPHERALS
 using GpioType = miosix::GpioPin;
 #else
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
 using GpioType = MockGpioPin;
 #endif
 
@@ -93,10 +93,10 @@ public:
     SPIBusInterface() {}
 
     ///< Delete copy/move contructors/operators.
-    SPIBusInterface(const SPIBusInterface&) = delete;
+    SPIBusInterface(const SPIBusInterface&)            = delete;
     SPIBusInterface& operator=(const SPIBusInterface&) = delete;
     SPIBusInterface(SPIBusInterface&&)                 = delete;
-    SPIBusInterface& operator=(SPIBusInterface&&) = delete;
+    SPIBusInterface& operator=(SPIBusInterface&&)      = delete;
 
     /**
      * @brief Configures the bus with the provided configuration parameters.
diff --git a/src/shared/drivers/spi/SPISlaveBus.h b/src/shared/drivers/spi/SPISlaveBus.h
index a530c7bd0cfc44e81dad78b60abd7d86764eb3c4..fcd274c25937ff768f112e0312176d0d3344024f 100644
--- a/src/shared/drivers/spi/SPISlaveBus.h
+++ b/src/shared/drivers/spi/SPISlaveBus.h
@@ -38,10 +38,10 @@ public:
     SPISlaveBus(SPIType* spi, SPISignalGenerator signalGenerator);
 
     ///< Delete copy/move contructors/operators.
-    SPISlaveBus(const SPISlaveBus&) = delete;
+    SPISlaveBus(const SPISlaveBus&)            = delete;
     SPISlaveBus& operator=(const SPISlaveBus&) = delete;
     SPISlaveBus(SPISlaveBus&&)                 = delete;
-    SPISlaveBus& operator=(SPISlaveBus&&) = delete;
+    SPISlaveBus& operator=(SPISlaveBus&&)      = delete;
 
     /**
      * @brief Configures and enables the bus with the provided configuration.
diff --git a/src/shared/drivers/spi/SPITransaction.h b/src/shared/drivers/spi/SPITransaction.h
index 3cc057519782f9a0abdc4ce96c9e0e440ad2ce33..6c98cafbe1b4ba524f8ade19e399acb4c2b61b47 100644
--- a/src/shared/drivers/spi/SPITransaction.h
+++ b/src/shared/drivers/spi/SPITransaction.h
@@ -84,10 +84,10 @@ public:
                    WriteBit writeBit = WriteBit::NORMAL);
 
     ///< Delete copy/move contructors/operators.
-    SPITransaction(const SPITransaction &) = delete;
+    SPITransaction(const SPITransaction &)            = delete;
     SPITransaction &operator=(const SPITransaction &) = delete;
     SPITransaction(SPITransaction &&)                 = delete;
-    SPITransaction &operator=(SPITransaction &&) = delete;
+    SPITransaction &operator=(SPITransaction &&)      = delete;
 
     /**
      * @brief Returns the underlying bus for low level access.
diff --git a/src/shared/drivers/timer/BasicTimer.h b/src/shared/drivers/timer/BasicTimer.h
index 1260739b7c1f3bc8f8357c102d623630ec8b43cf..57c8476a4170ec10823c1743162fa0e619821bc5 100644
--- a/src/shared/drivers/timer/BasicTimer.h
+++ b/src/shared/drivers/timer/BasicTimer.h
@@ -85,12 +85,14 @@ public:
     explicit BasicTimer(TIM_TypeDef *timer);
 
     /**
-     * @brief Resets the registers and disables the peripheral clock.
+     * @brief Disables the peripheral clock.
      */
     ~BasicTimer();
 
     TIM_TypeDef *getTimer();
 
+    uint8_t getTimerNumber();
+
     /**
      * @brief Resets the timer configuration to the default state.
      *
@@ -207,14 +209,44 @@ inline BasicTimer::BasicTimer(TIM_TypeDef *timer) : timer(timer)
     ClockUtils::enablePeripheralClock(timer);
 }
 
-inline BasicTimer::~BasicTimer()
-{
-    reset();
-    ClockUtils::disablePeripheralClock(timer);
-}
+inline BasicTimer::~BasicTimer() { ClockUtils::disablePeripheralClock(timer); }
 
 inline TIM_TypeDef *BasicTimer::getTimer() { return timer; }
 
+inline uint8_t BasicTimer::getTimerNumber()
+{
+    if (timer == TIM1)
+        return 1;
+    else if (timer == TIM2)
+        return 2;
+    else if (timer == TIM3)
+        return 3;
+    else if (timer == TIM4)
+        return 4;
+    else if (timer == TIM5)
+        return 5;
+    else if (timer == TIM6)
+        return 6;
+    else if (timer == TIM7)
+        return 7;
+    else if (timer == TIM8)
+        return 8;
+    else if (timer == TIM9)
+        return 9;
+    else if (timer == TIM10)
+        return 10;
+    else if (timer == TIM11)
+        return 11;
+    else if (timer == TIM12)
+        return 12;
+    else if (timer == TIM13)
+        return 13;
+    else if (timer == TIM14)
+        return 14;
+    else
+        return 0;
+}
+
 inline void BasicTimer::reset()
 {
     timer->CR1  = 0;
diff --git a/src/shared/drivers/timer/GeneralPurposeTimer.h b/src/shared/drivers/timer/GeneralPurposeTimer.h
index ca968af17cdf858d7224842373987a2be33d9690..de22562375a598275a56c8231ed9e8e5752cb5c7 100644
--- a/src/shared/drivers/timer/GeneralPurposeTimer.h
+++ b/src/shared/drivers/timer/GeneralPurposeTimer.h
@@ -110,7 +110,7 @@ public:
     explicit GeneralPurposeTimer(TIM_TypeDef *timer);
 
     /**
-     * @brief Resets the registers and disables the peripheral clock.
+     * @brief Disables the peripheral clock.
      */
     ~GeneralPurposeTimer();
 
@@ -230,7 +230,6 @@ inline GeneralPurposeTimer<T>::GeneralPurposeTimer(TIM_TypeDef *timer)
 template <typename T>
 inline GeneralPurposeTimer<T>::~GeneralPurposeTimer()
 {
-    reset();
     ClockUtils::disablePeripheralClock(timer);
 }
 
diff --git a/src/shared/drivers/timer/PWM.cpp b/src/shared/drivers/timer/PWM.cpp
index 471466913c65ab4e768d3b2d5cbea301d6e371fc..f8e2faa107ec05978202ac4d8725ef2ae8d2313c 100644
--- a/src/shared/drivers/timer/PWM.cpp
+++ b/src/shared/drivers/timer/PWM.cpp
@@ -30,8 +30,6 @@ PWM::PWM(TIM_TypeDef* const timer, unsigned int pwmFrequency,
     : timer(timer), pwmFrequency(pwmFrequency),
       dutyCycleResolution(dutyCycleResolution)
 {
-    // TODO: Enable the peripheral clock
-
     // Erase the previous timer configuration
     this->timer.reset();
 
@@ -39,11 +37,7 @@ PWM::PWM(TIM_TypeDef* const timer, unsigned int pwmFrequency,
     setTimerConfiguration();
 }
 
-PWM::~PWM()
-{
-    // TODO: Disable the peripheral clock
-    timer.reset();
-}
+PWM::~PWM() { timer.reset(); }
 
 void PWM::setFrequency(unsigned int pwmFrequency)
 {
@@ -90,10 +84,12 @@ void PWM::setDutyCycle(TimerUtils::Channel channel, float dutyCycle)
 
 float PWM::getDutyCycle(TimerUtils::Channel channel)
 {
-    return static_cast<float>(timer.readCaptureCompareRegister(channel) /
-                              timer.readAutoReloadRegister());
+    return static_cast<float>(timer.readCaptureCompareRegister(channel)) /
+           static_cast<float>(timer.readAutoReloadRegister());
 }
 
+GP16bitTimer PWM::getTimer() { return timer; }
+
 void PWM::setTimerConfiguration()
 {
     timer.disable();
diff --git a/src/shared/drivers/timer/PWM.h b/src/shared/drivers/timer/PWM.h
index 988613fa78738e2bbbd93d15c244c937cc3b2645..d30e5d45ca031a536acf963a4b50739d99d79b49 100644
--- a/src/shared/drivers/timer/PWM.h
+++ b/src/shared/drivers/timer/PWM.h
@@ -35,13 +35,13 @@ namespace Boardcore
  * when deleted) but no channels are enabled. This means that you will only need
  * to use the channels without worrying about the underlying timer.
  *
- * The PWM driver accepts a the pointer to the peripheral registers of a timer
- * and uses it as a 16bit general purpose timer. No checks are in place to this
+ * The PWM driver accepts a pointer to the peripheral registers of a timer and
+ * uses it as a 16bit general purpose timer. No checks are in place to this
  * pointer, thus make sure to pass a proper value!
  *
  * Moreover, even 32bit general purpose timers are used as if they where 16bit.
  * At the moment there is no need for further accuracy but if it ever will be,
- * exceeding this class is simble, just add a template parameter and pass it to
+ * exceeding this class is simple, just add a template parameter and pass it to
  * the GeneralPurposeTimer parameter.
  *
  * Check out the following spread sheet to visually see how the timers registers
@@ -95,6 +95,11 @@ public:
      */
     float getDutyCycle(TimerUtils::Channel channel);
 
+    /**
+     * @brief Returns the timer used to generate the pwm signal.
+     */
+    GP16bitTimer getTimer();
+
 private:
     // This class is not copyable!
     PWM& operator=(const PWM&) = delete;
diff --git a/src/shared/drivers/timer/TimerUtils.h b/src/shared/drivers/timer/TimerUtils.h
index b575dcb16a35b3a11e0f42e2f60aa2cf039ba1a6..7d619419f2c13d1b73f4ab649038b6e294c953ea 100644
--- a/src/shared/drivers/timer/TimerUtils.h
+++ b/src/shared/drivers/timer/TimerUtils.h
@@ -241,7 +241,7 @@ enum class OutputComparePolarity : uint16_t
     ACTIVE_LOW  = 0x1
 };
 
-enum class Channel : int
+enum class Channel : uint8_t
 {
     CHANNEL_1 = 0,
     CHANNEL_2 = 1,
@@ -254,7 +254,7 @@ enum class Channel : int
  *
  * @return Timer input clock, APB1 or ABP2.
  */
-ClockUtils::APB getTimerInputClock(TIM_TypeDef *timer);
+ClockUtils::APB getTimerInputClock(const TIM_TypeDef *timer);
 
 /**
  * @brief Returns the timer clock frequency before the prescaler.
@@ -264,7 +264,7 @@ ClockUtils::APB getTimerInputClock(TIM_TypeDef *timer);
  * @param timer Timer to use.
  * @return Prescaler input frequency.
  */
-uint32_t getPrescalerInputFrequency(TIM_TypeDef *timer);
+uint32_t getPrescalerInputFrequency(const TIM_TypeDef *timer);
 
 /**
  * @brief Return the timer clock frequency.
@@ -360,7 +360,7 @@ uint16_t computePrescalerValue(TIM_TypeDef *timer, int targetFrequency);
 
 }  // namespace TimerUtils
 
-inline ClockUtils::APB TimerUtils::getTimerInputClock(TIM_TypeDef *timer)
+inline ClockUtils::APB TimerUtils::getTimerInputClock(const TIM_TypeDef *timer)
 {
     // Timers can be connected to APB1 or APB2 clocks.
     // APB1: TIM2-7,12-15
@@ -377,7 +377,7 @@ inline ClockUtils::APB TimerUtils::getTimerInputClock(TIM_TypeDef *timer)
     }
 }
 
-inline uint32_t TimerUtils::getPrescalerInputFrequency(TIM_TypeDef *timer)
+inline uint32_t TimerUtils::getPrescalerInputFrequency(const TIM_TypeDef *timer)
 {
     return ClockUtils::getAPBFrequency(getTimerInputClock(timer));
 }
diff --git a/src/shared/drivers/timer/TimestampTimer.h b/src/shared/drivers/timer/TimestampTimer.h
index 3f54fb1ad6959892353230fce24cc75a5e813f5c..8b2bc6f2f445bc0a21f3fd32bd754be102759e25 100644
--- a/src/shared/drivers/timer/TimestampTimer.h
+++ b/src/shared/drivers/timer/TimestampTimer.h
@@ -101,7 +101,7 @@ private:
 inline uint64_t TimestampTimer::getTimestamp()
 {
 #ifdef COMPILE_FOR_HOST
-    return 0;
+    return time(NULL);
 #else
     // With a timer frequency of 250KHz, the conversion from timer ticks to
     // microseconds only take a 2 byte shift (x4)
diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc5612a0855997b158c2a1cc5f35c2b01248ce94
--- /dev/null
+++ b/src/shared/drivers/usart/USART.cpp
@@ -0,0 +1,780 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "drivers/usart/USART.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <utils/Debug.h>
+
+#include <string>
+
+#include "arch/common/drivers/serial.h"
+#include "filesystem/file_access.h"
+#include "miosix.h"
+
+Boardcore::USART *Boardcore::USART::ports[N_USART_PORTS];
+
+/**
+ * \internal Interrupt routine for usart1 actual implementation.
+ */
+void __attribute__((used)) usart1irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[0];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[0];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for usart1.
+ */
+void __attribute__((naked, used)) USART1_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z22usart1irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for usart2 actual implementation.
+ */
+void __attribute__((used)) usart2irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[1];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[1];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for usart2.
+ */
+void __attribute__((naked, used)) USART2_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z22usart2irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for usart3 actual implementation.
+ */
+void __attribute__((used)) usart3irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[2];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[2];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for usart3.
+ */
+void __attribute__((naked, used)) USART3_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z22usart3irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for uart4 actual implementation.
+ */
+void __attribute__((used)) uart4irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[3];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[3];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for uart4.
+ */
+void __attribute__((naked, used)) UART4_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z21uart4irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for uart5 actual implementation.
+ */
+void __attribute__((used)) uart5irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[4];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[4];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for uart5.
+ */
+void __attribute__((naked, used)) UART5_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z21uart5irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for usart6 actual implementation.
+ */
+void __attribute__((used)) usart6irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[5];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[5];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for usart6.
+ */
+void __attribute__((naked, used)) USART6_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z22usart6irqImplBoardcorev");
+    restoreContext();
+}
+
+#ifdef STM32F429xx
+
+/**
+ * \internal Interrupt routine for uart7 actual implementation.
+ */
+void __attribute__((used)) uart7irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[6];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[6];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for uart7.
+ */
+void __attribute__((naked, used)) UART7_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z21uart7irqImplBoardcorev");
+    restoreContext();
+}
+
+/**
+ * \internal Interrupt routine for uart8 actual implementation.
+ */
+void __attribute__((used)) uart8irqImplBoardcore()
+{
+    Boardcore::USART *port_boardcore = Boardcore::USART::ports[7];
+    if (port_boardcore)
+        port_boardcore->IRQhandleInterrupt();
+    else
+    {
+        miosix::STM32Serial *port = miosix::STM32Serial::ports[7];
+        if (port)
+            port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * \internal Interrupt routine for uart8.
+ */
+void __attribute__((naked, used)) UART8_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z21uart8irqImplBoardcorev");
+    restoreContext();
+}
+
+#endif  // STM32F429xx
+
+namespace Boardcore
+{
+
+USARTInterface::~USARTInterface() {}
+
+bool USARTInterface::initPins(miosix::GpioPin tx, int nAFtx, miosix::GpioPin rx,
+                              int nAFrx)
+{
+    if (pinInitialized)
+    {
+        return false;
+    }
+
+    miosix::FastInterruptDisableLock dLock;
+
+    this->tx = tx;
+    this->rx = rx;
+
+    tx.mode(miosix::Mode::ALTERNATE);
+    tx.alternateFunction(nAFtx);
+
+    rx.mode(miosix::Mode::ALTERNATE);
+    rx.alternateFunction(nAFrx);
+
+    pinInitialized = true;
+    return true;
+}
+
+void USART::IRQhandleInterrupt()
+{
+    char c;
+
+    // If read data register is empty then read data
+    if (usart->SR & USART_SR_RXNE)
+    {
+        // Always read data, since this clears interrupt flags
+        c = usart->DR;
+        // If no error put data in buffer
+        if (!(usart->SR & USART_SR_FE))
+            if (rxQueue.tryPut(c) == false)  // FIFO overflow
+                ;
+
+        idle = false;
+    }
+
+    if (usart->SR & USART_SR_IDLE)
+        idle = true;
+
+    if (usart->SR & USART_SR_IDLE || rxQueue.size() >= rxQueue.capacity() / 2)
+    {
+        c = usart->DR;  // Clears interrupt flags
+
+        // Enough data in buffer or idle line, awake thread
+        if (rxWaiting)
+        {
+            rxWaiting->IRQwakeup();
+            rxWaiting = 0;
+        }
+    }
+}
+
+USART::USART(USARTType *usart, Baudrate baudrate, unsigned int queueLen)
+    : rxQueue(queueLen)
+{
+    // Setting the id of the serial port
+    switch (reinterpret_cast<uint32_t>(usart))
+    {
+        case USART1_BASE:
+            this->id = 1;
+            initPins(u1tx1::getPin(), 7, u1rx1::getPin(), 7);
+            irqn = USART1_IRQn;
+            break;
+        case USART2_BASE:
+            this->id = 2;
+            initPins(u2tx1::getPin(), 7, u2rx1::getPin(), 7);
+            irqn = USART2_IRQn;
+            break;
+        case USART3_BASE:
+            this->id = 3;
+            initPins(u3tx1::getPin(), 7, u3rx1::getPin(), 7);
+            irqn = USART3_IRQn;
+            break;
+        case UART4_BASE:
+            this->id = 4;
+            initPins(u4tx1::getPin(), 8, u4rx1::getPin(), 8);
+            irqn = UART4_IRQn;
+            break;
+        case UART5_BASE:
+            this->id = 5;
+            initPins(u5tx::getPin(), 8, u5rx::getPin(), 8);
+            irqn = UART5_IRQn;
+            break;
+        case USART6_BASE:
+            this->id = 6;
+            initPins(u6tx1::getPin(), 8, u6rx1::getPin(), 8);
+            irqn = USART6_IRQn;
+            break;
+#ifdef STM32F429xx
+        case UART7_BASE:
+            this->id = 7;
+            initPins(u7tx1::getPin(), 8, u7rx1::getPin(), 8);
+            irqn = UART7_IRQn;
+            break;
+        case UART8_BASE:
+            this->id = 8;
+            initPins(u8tx::getPin(), 8, u8rx::getPin(), 8);
+            irqn = UART8_IRQn;
+            break;
+#endif  // STM32F429xx
+    }
+
+    commonConstructor(usart, baudrate);
+}
+
+USART::USART(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx,
+             miosix::GpioPin rx, unsigned int queueLen)
+    : rxQueue(queueLen)
+{
+    // Setting the id of the serial port
+    switch (reinterpret_cast<uint32_t>(usart))
+    {
+        case USART1_BASE:
+            this->id = 1;
+            initPins(tx, 7, rx, 7);
+            irqn = USART1_IRQn;
+            break;
+        case USART2_BASE:
+            this->id = 2;
+            initPins(tx, 7, rx, 7);
+            irqn = USART2_IRQn;
+            break;
+        case USART3_BASE:
+            this->id = 3;
+            initPins(tx, 7, rx, 7);
+            irqn = USART3_IRQn;
+            break;
+        case UART4_BASE:
+            this->id = 4;
+            initPins(tx, 8, rx, 8);
+            irqn = UART4_IRQn;
+            break;
+        case UART5_BASE:
+            this->id = 5;
+            initPins(tx, 8, rx, 8);
+            irqn = UART5_IRQn;
+            break;
+        case USART6_BASE:
+            this->id = 6;
+            initPins(tx, 8, rx, 8);
+            irqn = USART6_IRQn;
+            break;
+#ifdef STM32F429xx
+        case UART7_BASE:
+            this->id = 7;
+            initPins(tx, 8, rx, 8);
+            irqn = UART7_IRQn;
+            break;
+        case UART8_BASE:
+            this->id = 8;
+            initPins(tx, 8, rx, 8);
+            irqn = UART8_IRQn;
+            break;
+#endif  // STM32F429xx
+    }
+
+    commonConstructor(usart, baudrate);
+}
+
+void USART::commonConstructor(USARTType *usart, Baudrate baudrate)
+{
+    this->usart = usart;
+
+    // Enabling the peripehral on the right APB
+    ClockUtils::enablePeripheralClock(usart);
+    RCC_SYNC();
+
+    // Enabling the usart peripheral
+    {
+        miosix::FastInterruptDisableLock dLock;
+        usart->CR1 |= USART_CR1_UE;
+    }
+
+    // Setting the baudrate chosen
+    setBaudrate(baudrate);
+
+    // Default settings
+    setStopBits(1);
+    setWordLength(USART::WordLength::BIT8);
+    setParity(USART::ParityBit::NO_PARITY);
+    setOversampling(false);
+}
+
+USART::~USART()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Take out the usart object we are going to destruct
+        USART::ports[this->id - 1] = nullptr;
+
+        // Disabling the usart
+        usart->CR1 &= ~(USART_CR1_UE | USART_CR1_TE | USART_CR1_RE);
+
+        // Disabling the interrupt of the serial port
+        NVIC_DisableIRQ(irqn);
+    }
+}
+
+bool USART::init()
+{
+    if (id < 1 || id > MAX_SERIAL_PORTS || !pinInitialized)
+    {
+        TRACE("Not supported USART id or pins not initialized\n");
+        return false;
+    }
+
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable usart, receiver, receiver interrupt and idle interrupt
+        usart->CR1 |= USART_CR1_RXNEIE    // Interrupt on data received
+                      | USART_CR1_IDLEIE  // interrupt on idle line
+                      | USART_CR1_TE      // Transmission enabled
+                      | USART_CR1_RE;     // Reception enabled
+
+        // Sample only one bit
+        usart->CR3 |= USART_CR3_ONEBIT;
+
+        // Enabling the interrupt for the relative serial port
+        NVIC_SetPriority(irqn, 15);
+        NVIC_EnableIRQ(irqn);
+
+        // Add to the array of usarts so that the interrupts can see it
+        USART::ports[id - 1] = this;
+    }
+
+    // Clearing the queue for random data read at the beginning
+    miosix::Thread::sleep(1);
+    this->clearQueue();
+
+    return true;
+}
+
+void USART::setWordLength(WordLength wordLength)
+{
+
+    miosix::FastInterruptDisableLock dLock;
+    (wordLength == WordLength::BIT8 ? usart->CR1 &= ~USART_CR1_M
+                                    : usart->CR1 |= USART_CR1_M);
+    this->wordLength = wordLength;
+}
+
+void USART::setParity(ParityBit parity)
+{
+    miosix::FastInterruptDisableLock dLock;
+    (parity == ParityBit::NO_PARITY ? usart->CR1 &= ~USART_CR1_PCE
+                                    : usart->CR1 |= USART_CR1_PCE);
+    this->parity = parity;
+}
+
+void USART::setStopBits(int stopBits)
+{
+    miosix::FastInterruptDisableLock dLock;
+    this->stopBits = stopBits;
+    usart->CR2 &= ~USART_CR2_STOP;
+    if (stopBits == 2)
+    {
+        usart->CR2 |= USART_CR2_STOP_1;
+    }
+}
+
+void USART::setOversampling(bool oversampling)
+{
+    miosix::FastInterruptDisableLock dLock;
+    this->over8 = oversampling;
+    (oversampling ? usart->CR1 |= USART_CR1_OVER8
+                  : usart->CR1 &= ~USART_CR1_OVER8);
+}
+
+void USART::setBaudrate(Baudrate baudrate)
+{
+    /*
+     * Baudrate setting:
+     * fixed point: mantissa first 12 bits, other 4 bits are the fixed point
+     * value of the fraction.
+     * - if over8==0 => DIV_Fraction[3:0] (all fraction used)
+     * USART_DIV = f/(16*baud)
+     * - if over8==1 => 0+DIV_Fraction[2:0] (first bit of fraction is 0)
+     * USART_DIV = f/(8*baud)
+     */
+    miosix::InterruptDisableLock dLock;
+
+    // USART1 and USART6 is always connected to the APB2, while all the others
+    // UART/USART peripherals are always connected to APB1
+    uint32_t f = ClockUtils::getAPBFrequency(
+        (id == 1 || id == 6 ? ClockUtils::APB::APB2     // High speed APB2
+                            : ClockUtils::APB::APB1));  // Low speed APB1,
+
+    // <<4 in order to shift to left of 4 positions, to create a fixed point
+    // number of 4 decimal digits /8 == >>3 in order to divide per 8 (from the
+    // formula in the datasheet)
+    uint32_t USART_DIV = ((f << 1) / (((int)baudrate * (over8 ? 1 : 2))));
+
+    // rounding to the nearest
+    uint32_t brr = (USART_DIV / 2) + (USART_DIV & 1);
+
+    if (over8)
+    {
+        brr += brr & 1;
+    }
+
+    usart->BRR = brr;
+
+    this->baudrate = baudrate;
+}
+
+int USART::read(void *buffer, size_t nBytes)
+{
+    miosix::Lock<miosix::FastMutex> l(rxMutex);
+
+    char *buf     = reinterpret_cast<char *>(buffer);
+    size_t result = 0;
+    miosix::FastInterruptDisableLock dLock;
+    for (;;)
+    {
+        // Try to get data from the queue
+        for (; result < nBytes; result++)
+        {
+            if (rxQueue.tryGet(buf[result]) == false)
+                break;
+            // This is here just not to keep IRQ disabled for the whole loop
+            miosix::FastInterruptEnableLock eLock(dLock);
+        }
+
+        // Not checking if the nBytes read are more than 0
+        if (result == nBytes || (idle && result > 0))
+            break;
+
+        // Wait for data in the queue
+        do
+        {
+            rxWaiting = miosix::Thread::IRQgetCurrentThread();
+            miosix::Thread::IRQwait();
+            {
+                miosix::FastInterruptEnableLock eLock(dLock);
+                miosix::Thread::yield();
+            }
+        } while (rxWaiting);
+    }
+
+    return result;
+}
+
+int USART::write(void *buffer, size_t nBytes)
+{
+    miosix::Lock<miosix::FastMutex> l(txMutex);
+
+    // TODO: Use the send complete interrupt in order not to have a busy while
+    // loop waiting
+    const char *buf = reinterpret_cast<const char *>(buffer);
+    size_t i        = 0;
+    for (i = 0; i < nBytes; i++)
+    {
+        while ((usart->SR & USART_SR_TXE) == 0)
+            ;
+
+        usart->DR = *buf;
+        buf++;
+    }
+
+    return i;
+}
+
+int USART::writeString(const char *buffer)
+{
+    int i = 0;
+    miosix::Lock<miosix::FastMutex> l(txMutex);
+
+    // Send everything, also the ending '\0' character
+    usart->DR = *buffer;
+    i++;
+    while (*buffer != '\0')
+    {
+        buffer++;
+        while (!(usart->SR & USART_SR_TXE))
+            ;
+
+        usart->DR = *buffer;
+        i++;
+    };
+
+    return i;
+}
+
+void USART::clearQueue() { rxQueue.reset(); }
+
+STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate)
+{
+    this->usart    = usart;
+    this->baudrate = baudrate;
+    switch (reinterpret_cast<uint32_t>(usart))
+    {
+        case USART1_BASE:
+            this->id = 1;
+            initPins(u1tx1::getPin(), 7, u1rx1::getPin(), 7);
+            this->serialPortName = std::string("usart1");
+            break;
+        case USART2_BASE:
+            this->id = 2;
+            initPins(u2tx1::getPin(), 7, u2rx1::getPin(), 7);
+            this->serialPortName = std::string("usart2");
+            break;
+        case USART3_BASE:
+            this->id = 3;
+            initPins(u3tx1::getPin(), 7, u3rx1::getPin(), 7);
+            this->serialPortName = std::string("usart3");
+            break;
+    }
+    initialized = false;
+    fd          = -1;
+}
+
+STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate,
+                                       miosix::GpioPin tx, miosix::GpioPin rx)
+{
+    this->usart    = usart;
+    this->baudrate = baudrate;
+    switch (reinterpret_cast<uint32_t>(usart))
+    {
+        case USART1_BASE:
+            this->id             = 1;
+            this->serialPortName = std::string("usart1");
+            break;
+        case USART2_BASE:
+            this->id             = 2;
+            this->serialPortName = std::string("usart2");
+            break;
+        case USART3_BASE:
+            this->id             = 3;
+            this->serialPortName = std::string("usart3");
+            break;
+    }
+    initPins(tx, 7, rx, 7);
+    initialized = false;
+    fd          = -1;
+}
+
+STM32SerialWrapper::~STM32SerialWrapper()
+{
+    miosix::intrusive_ref_ptr<miosix::DevFs> devFs =
+        miosix::FilesystemManager::instance().getDevFs();
+    close(fd);
+    devFs->remove(serialPortName.c_str());
+}
+
+bool STM32SerialWrapper::init()
+{
+    if (id > 3)
+    {
+        TRACE(
+            "[STM32SerialWrapper] USART id greater than 3 is not supported\n");
+        return false;
+    }
+
+    if (initialized)
+    {
+        TRACE(
+            "[STM32SerialWrapper] Error : serial communication already "
+            "initialized!\n");
+        return false;
+    }
+    else if (!serialCommSetup())
+    {
+        TRACE(
+            "[STM32SerialWrapper] Error : can't initialize serial "
+            "communication!\n");
+        return false;
+    }
+
+    initialized = true;
+    return true;
+}
+
+bool STM32SerialWrapper::serialCommSetup()
+{
+    // Creates and adds the serial port to the devices
+    if (!pinInitialized)
+        serial = new miosix::STM32Serial(id, static_cast<int>(baudrate));
+    else
+    {
+        serial =
+            new miosix::STM32Serial(id, static_cast<int>(baudrate), tx, rx);
+    }
+
+    // Adds a device to the file system
+    if (!miosix::FilesystemManager::instance().getDevFs()->addDevice(
+            serialPortName.c_str(),
+            miosix::intrusive_ref_ptr<miosix::Device>(serial)))
+        return false;
+
+    // Path string "/dev/<name_of_port>" for the port we want to open
+    std::string serialPortPath = "/dev/" + serialPortName;
+
+    // Open serial port
+    fd = open(serialPortPath.c_str(), O_RDWR);
+
+    if (fd <= -1)
+    {
+        TRACE("Cannot open %s\n", serialPortPath.c_str());
+        return false;
+    }
+
+    return true;
+}
+
+int STM32SerialWrapper::writeString(const char *data)
+{
+    // strlen + 1 in order to send the '/0' terminated string
+    return ::write(fd, data, strlen(data) + 1);
+}
+
+int STM32SerialWrapper::write(void *buf, size_t nChars)
+{
+    return ::write(fd, buf, nChars);
+}
+
+int STM32SerialWrapper::read(void *buf, size_t nBytes)
+{
+    return ::read(fd, buf, nBytes);
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/drivers/usart/USART.h b/src/shared/drivers/usart/USART.h
new file mode 100644
index 0000000000000000000000000000000000000000..05cee3a1662984393fc8790c31dab329eb1f201c
--- /dev/null
+++ b/src/shared/drivers/usart/USART.h
@@ -0,0 +1,426 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <interfaces/arch_registers.h>
+#include <miosix.h>
+#include <utils/ClockUtils.h>
+
+#include <string>
+
+#include "arch/common/drivers/serial.h"
+
+#ifndef USE_MOCK_PERIPHERALS
+using USARTType = USART_TypeDef;
+#else
+// TODO: Create test utils
+#endif
+
+#ifdef STM32F429xx
+#define N_USART_PORTS 8
+#else
+#define N_USART_PORTS 6
+#endif
+
+// A nice feature of the stm32 is that the USART are connected to the same
+// GPIOS in all families, stm32f1, f2, f4 and l1. Additionally, USART1 and
+// USART6 are always connected to the APB2, while the other USART/UARTs are
+// connected to the APB1.
+
+// USART1: AF7
+typedef miosix::Gpio<GPIOB_BASE, 6> u1tx1;
+typedef miosix::Gpio<GPIOB_BASE, 7> u1rx1;
+typedef miosix::Gpio<GPIOA_BASE, 9> u1tx2;
+typedef miosix::Gpio<GPIOA_BASE, 10> u1rx2;
+// typedef miosix::Gpio<GPIOA_BASE, 11> u1cts;
+// typedef miosix::Gpio<GPIOA_BASE, 12> u1rts;
+
+// USART2: AF7
+typedef miosix::Gpio<GPIOA_BASE, 2> u2tx1;
+typedef miosix::Gpio<GPIOA_BASE, 3> u2rx1;
+typedef miosix::Gpio<GPIOD_BASE, 5> u2tx2;
+typedef miosix::Gpio<GPIOD_BASE, 6> u2rx2;
+// typedef miosix::Gpio<GPIOA_BASE, 0> u2cts;
+// typedef miosix::Gpio<GPIOA_BASE, 1> u2rts;
+
+// USART3: AF7
+typedef miosix::Gpio<GPIOB_BASE, 10> u3tx1;
+typedef miosix::Gpio<GPIOB_BASE, 11> u3rx1;
+typedef miosix::Gpio<GPIOD_BASE, 8> u3tx2;
+typedef miosix::Gpio<GPIOD_BASE, 9> u3rx2;
+// typedef miosix::Gpio<GPIOB_BASE, 13> u3cts;
+// typedef miosix::Gpio<GPIOB_BASE, 14> u3rts;
+
+// UART4: AF8
+typedef miosix::Gpio<GPIOA_BASE, 0> u4tx1;
+typedef miosix::Gpio<GPIOA_BASE, 1> u4rx1;
+typedef miosix::Gpio<GPIOC_BASE, 10> u4tx2;
+typedef miosix::Gpio<GPIOC_BASE, 11> u4rx2;
+
+// UART5: AF8
+typedef miosix::Gpio<GPIOC_BASE, 12> u5tx;
+typedef miosix::Gpio<GPIOD_BASE, 2> u5rx;
+
+// USART6: AF8
+typedef miosix::Gpio<GPIOC_BASE, 6> u6tx1;
+typedef miosix::Gpio<GPIOC_BASE, 7> u6rx1;
+#ifdef STM32F429xx
+typedef miosix::Gpio<GPIOG_BASE, 14> u6tx2;
+typedef miosix::Gpio<GPIOG_BASE, 9> u6rx2;
+
+// USART7: AF8
+typedef miosix::Gpio<GPIOE_BASE, 8> u7tx1;
+typedef miosix::Gpio<GPIOE_BASE, 7> u7rx1;
+typedef miosix::Gpio<GPIOF_BASE, 7> u7tx2;
+typedef miosix::Gpio<GPIOF_BASE, 6> u7rx2;
+
+// USART8: AF8
+typedef miosix::Gpio<GPIOE_BASE, 1> u8tx;
+typedef miosix::Gpio<GPIOE_BASE, 0> u8rx;
+#endif  // STM32F429xx
+
+namespace Boardcore
+{
+/**
+ * @brief Abstract class that implements the interface for the USART/UART serial
+ * communication.
+ */
+class USARTInterface
+{
+public:
+    enum class Baudrate : int
+    {
+        // B1200   = 1200, // NOT WORKING WITH 1200 baud
+        B2400   = 2400,
+        B9600   = 9600,
+        B19200  = 19200,
+        B38400  = 38400,
+        B57600  = 57600,
+        B115200 = 115200,
+        B230400 = 230400,
+        B460800 = 460800,
+        B921600 = 921600
+    };
+
+    virtual ~USARTInterface() = 0;
+
+    /**
+     * @brief Initializes the peripheral enabling his interrupts, the interrupts
+     * in the NVIC.
+     *
+     * All the setup phase (with the setting of the pins and their alternate
+     * functions) must be done before the initialization of the peripheral.
+     */
+    virtual bool init() = 0;
+
+    /**
+     * @brief Blocking read operation to read nBytes or till the data transfer
+     * is complete.
+     */
+    virtual int read(void *buffer, size_t nBytes) = 0;
+
+    /**
+     * @brief Blocking write operation.
+     */
+    virtual int write(void *buf, size_t nChars) = 0;
+
+    /**
+     * @brief Write a string to the serial, comprising the '\0' character.
+     */
+    virtual int writeString(const char *buffer) = 0;
+
+    /**
+     * @brief Returns the id of the serial.
+     */
+    int getId() { return id; };
+
+protected:
+    /**
+     * @brief Initializes the pins with the appropriate alternate functions.
+     *
+     * @param tx Tranmission pin.
+     * @param nAFtx Tranmission pin alternate function.
+     * @param rx Reception pin.
+     * @param nAFrx Reception pin alternate function.
+     */
+    bool initPins(miosix::GpioPin tx, int nAFtx, miosix::GpioPin rx, int nAFrx);
+
+    ///< True if initPins() already called successfully, false otherwise
+    bool pinInitialized = false;
+
+    miosix::GpioPin tx{GPIOA_BASE, 0};
+    miosix::GpioPin rx{GPIOA_BASE, 0};
+
+    USARTType *usart;
+    int id           = 1;  ///< Can be 1, 2, 3, 4, 5, 6, 7, 8
+    bool initialized = false;
+    Baudrate baudrate;  ///< Baudrate of the serial communication
+};
+
+/**
+ * @brief Driver for STM32F4 low level USART/UART peripheral.
+ *
+ * It allows to configure some low level parameters such as word length, parity,
+ * stop bits and oversampling.
+ */
+class USART : public USARTInterface
+{
+public:
+    enum class WordLength : bool
+    {
+        BIT8 = 0,
+        BIT9 = 1
+    };
+
+    enum class ParityBit : bool
+    {
+        NO_PARITY = 0,
+        PARITY    = 1
+    };
+
+    ///< Pointer to serial port classes to let interrupts access the classes
+    static USART *ports[];
+
+    /**
+     * @brief Interrupt handler that deals with receive and idle interrupts.
+     *
+     * Used to implement the reads, fills up a buffer when the interrupt is
+     * fired.
+     */
+    void IRQhandleInterrupt();
+
+    /**
+     * @brief Automatically enables the peripheral and timer peripheral clock.
+     *
+     * Sets the default values for all the parameters (1 stop bit, 8 bit data,
+     * no control flow and no oversampling).
+     * Initializes the serial port using the default pins, which are:
+     * - USART1: tx=PA9  rx=PA10
+     * - USART2: tx=PA2  rx=PA3
+     * - USART3: tx=PB10 rx=PB11
+     * - UART4:  tx=PA0  rx=PA1
+     * - UART5:  tx=PC12 rx=PD2
+     * - USART6: tx=PC6  rx=PC7
+     * - UART7:  tx=PE8  rx=PE7
+     * - UART8:  tx=PE1  rx=PE0
+     *
+     * @param usart structure that represents the usart peripheral [accepted
+     * are: USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8].
+     * @param baudrate member of the enum Baudrate that represents the baudrate
+     * with which the communication will take place.
+     */
+    USART(USARTType *usart, Baudrate baudrate,
+          unsigned int queueLen = usart_queue_default_capacity);
+
+    /**
+     * @brief Automatically enables the peripheral and timer peripheral clock.
+     *
+     * Sets the default values for all the parameters (1 stop bit, 8 bit data,
+     * no control flow and no oversampling).
+     * Initializes the serial port using custom pins.
+     *
+     * @param usart Structure that represents the usart peripheral [accepted
+     * are: USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8].
+     * @param baudrate Member of the enum Baudrate that represents the baudrate
+     * with which the communication will take place.
+     * @param tx Tranmission pin.
+     * @param rx Reception pin.
+     */
+    USART(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx,
+          miosix::GpioPin rx,
+          unsigned int queueLen = usart_queue_default_capacity);
+
+    /**
+     * @brief Disables the flags for the generation of the interrupts, the IRQ
+     * from the NVIC, the peripheral and removes his pointer from the ports
+     * list.
+     */
+    ~USART();
+
+    /**
+     * @brief Initializes the peripheral enabling his interrupts, the interrupts
+     * in the NVIC and setting the pins with the appropriate alternate
+     * functions.
+     *
+     * All the setup phase must be done before the initialization of the
+     * peripheral. The pins must be initialized before calling this function.
+     */
+    bool init();
+
+    /**
+     * @brief Blocking read operation to read nBytes or till the data transfer
+     * is complete.
+     */
+    int read(void *buffer, size_t nBytes);
+
+    /**
+     * @brief Blocking write operation.
+     */
+    int write(void *buf, size_t nChars);
+
+    /**
+     * @brief Write a string to the serial, comprising the '\0' character.
+     */
+    int writeString(const char *buffer);
+
+    /**
+     * @brief Set the length of the word to 8 or to 9.
+     *
+     * @param wl WordLength element that represents the length of the word.
+     */
+    void setWordLength(WordLength wl);
+
+    /**
+     * @brief Set the presence of the parity in the data sent.
+     *
+     * @param pb ParityBit element that represents the presence of the parity
+     * bit.
+     */
+    void setParity(ParityBit pb);
+
+    /**
+     * @brief Set the number of stop bits.
+     *
+     * @param stopBits number of stop bits [1,2].
+     */
+    void setStopBits(int stopBits);
+
+    /**
+     * @brief Set the baudrate in the BRR register.
+     *
+     * @param pb Baudrate element that represents the baudrate.
+     */
+    void setBaudrate(Baudrate br);
+
+    /**
+     * @brief Sets the Over8 bit.
+     *
+     * If it is set, the speed is increased; If it is reset the tolerance is
+     * increased.
+     */
+    void setOversampling(bool oversampling);
+
+    /**
+     * @brief Clears the rxQueue.
+     */
+    void clearQueue();
+
+private:
+    void commonConstructor(USARTType *usart, Baudrate baudrate);
+
+    IRQn_Type irqn;
+    miosix::FastMutex rxMutex;  ///< mutex for receiving on serial
+    miosix::FastMutex txMutex;  ///< mutex for transmitting on serial
+
+    ///< Pointer to the waiting on receive thread
+    miosix::Thread *rxWaiting = 0;
+
+    miosix::DynUnsyncQueue<char> rxQueue;  ///< Receiving queue
+    bool idle             = true;          ///< Receiver idle
+    ParityBit parity      = ParityBit::NO_PARITY;
+    WordLength wordLength = WordLength::BIT8;
+    int stopBits          = 1;      ///< Number of stop bits [1,2]
+    bool over8            = false;  ///< Oversalmpling 8 bit
+
+    ///< Default queue length
+    const static unsigned int usart_queue_default_capacity = 256;
+};
+
+/**
+ * @brief Wrapper for the STM32Serial driver in miosix.
+ */
+class STM32SerialWrapper : public USARTInterface
+{
+public:
+    /**
+     * @brief Initializes the serialPortName and initializes the default pins,
+     * which are:
+     * - USART1: tx=PA9  rx=PA10
+     * - USART2: tx=PA2  rx=PA3
+     * - USART3: tx=PB10 rx=PB11
+     * @param usart structure that represents the usart peripheral [accepted
+     * are: USART1, USART2, USART3].
+     * @param baudrate member of the enum Baudrate that represents the baudrate
+     * with which the communication will take place.
+     */
+    STM32SerialWrapper(USARTType *usart, Baudrate baudrate);
+
+    /**
+     * @brief Initializes the serialPortName and initializes the serial port
+     * using custom pins.
+     * @param usart structure that represents the usart peripheral [accepted
+     * are: USART1, USART2, USART3].
+     * @param baudrate member of the enum Baudrate that represents the baudrate
+     * with which the communication will take place.
+     * @param tx Tranmission pin
+     * @param rx Reception pin
+     */
+    STM32SerialWrapper(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx,
+                       miosix::GpioPin rx);
+
+    /**
+     * @brief Removes the device from the list of the devices and closes the
+     * file of the device.
+     */
+    ~STM32SerialWrapper();
+
+    /**
+     * @brief Initializes the peripheral.
+     *
+     * @see{STM32SerialWrapper::serialCommSetup}
+     */
+    bool init();
+
+    /**
+     * @brief Blocking read operation to read nBytes or till the data transfer
+     * is complete.
+     */
+    int read(void *buffer, size_t nBytes);
+
+    /**
+     * @brief Blocking write operation.
+     */
+    int write(void *buf, size_t nChars);
+
+    /**
+     * @brief Write a string to the serial, comprising the '\0' character.
+     */
+    int writeString(const char *buffer);
+
+private:
+    /**
+     * @brief Creates a device that represents the serial port, adds it to the
+     * file system and opens the file that represents the device.
+     */
+    bool serialCommSetup();
+
+    miosix::STM32Serial *serial;  ///< Pointer to the serial object
+
+    //< Port name of the port that has to be created for the communication
+    std::string serialPortName;
+
+    ///< File descriptor of the serial port file opened for transmission
+    int fd;
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/events/Event.h b/src/shared/events/Event.h
index d35c6ab219a2d5c2c7e0060e75eece7ea2eeb075..c38ec3ef83a50bbbe86428bbb8f7d9e89a363427 100644
--- a/src/shared/events/Event.h
+++ b/src/shared/events/Event.h
@@ -27,38 +27,27 @@
 namespace Boardcore
 {
 
-enum EventSignal : uint8_t
+typedef uint8_t Event;
+
+enum BasicEvent : Event
 {
     EV_ENTRY        = 0,
     EV_EXIT         = 1,
     EV_EMPTY        = 2,
     EV_INIT         = 3,
-    EV_FIRST_SIGNAL = 4
+    EV_FIRST_CUSTOM = 4
 };
 
 /**
- * 	Example definiton of custom signals:
-
-        enum CustomSignal : uint8_t
-        {
-                SIG_ONE = SG_FIRST_SIGNAL,
-                SIG_TWO,
-                SIG_THREE,
-                SIG_FOUR
-        };
-
+ * Example definition of custom events:
+ *
+ * enum CustomEvent : Event
+ * {
+ *     EV_ONE = EV_FIRST_CUSTOM,
+ *     EV_TWO,
+ *     EV_THREE.
+ *     EV_FOUR
+ * }
  */
 
-struct Event
-{
-    uint8_t code;
-};
-
-/* Example of extended Event structure
-
-        struct ExtendedEvent : public Event{
-                uint32_t customMember;
-        };
-*/
-
 }  // namespace Boardcore
diff --git a/src/shared/events/EventBroker.cpp b/src/shared/events/EventBroker.cpp
index 019aefc8e0464bd9ab3b88e4e3cb27cf329e1d0c..b748fc1e91393133da2709cab2a761fe341ed442 100644
--- a/src/shared/events/EventBroker.cpp
+++ b/src/shared/events/EventBroker.cpp
@@ -29,11 +29,10 @@ namespace Boardcore
 {
 
 EventBroker::EventBroker() {}
-
 void EventBroker::post(const Event& ev, uint8_t topic)
 {
 #ifdef TRACE_EVENTS
-    LOG_DEBUG(logger, "Event: {}, Topic: {}", ev.code, topic);
+    LOG_DEBUG(logger, "Event: {}, Topic: {}", ev, topic);
 #endif
 
     Lock<FastMutex> lock(mtxSubscribers);
@@ -66,6 +65,34 @@ void EventBroker::removeDelayed(uint16_t id)
     }
 }
 
+void EventBroker::subscribe(EventHandlerBase* subscriber, uint8_t topic)
+{
+    Lock<FastMutex> lock(mtxSubscribers);
+    subscribers[topic].push_back(subscriber);
+}
+
+void EventBroker::unsubscribe(EventHandlerBase* subscriber, uint8_t topic)
+{
+    Lock<FastMutex> lock(mtxSubscribers);
+
+    deleteSubscriber(subscribers.at(topic), subscriber);
+}
+
+void EventBroker::unsubscribe(EventHandlerBase* subscriber)
+{
+    Lock<FastMutex> lock(mtxSubscribers);
+    for (auto it = subscribers.begin(); it != subscribers.end(); it++)
+    {
+        deleteSubscriber(it->second, subscriber);
+    }
+}
+
+void EventBroker::clearDelayedEvents()
+{
+    Lock<FastMutex> lock(mtxDelayedEvents);
+    delayedEvents.clear();
+}
+
 // Posts delayed events with expired deadline
 void EventBroker::run()
 {
@@ -110,28 +137,6 @@ void EventBroker::run()
     }
 }
 
-void EventBroker::subscribe(EventHandlerBase* subscriber, uint8_t topic)
-{
-    Lock<FastMutex> lock(mtxSubscribers);
-    subscribers[topic].push_back(subscriber);
-}
-
-void EventBroker::unsubscribe(EventHandlerBase* subscriber, uint8_t topic)
-{
-    Lock<FastMutex> lock(mtxSubscribers);
-
-    deleteSubscriber(subscribers.at(topic), subscriber);
-}
-
-void EventBroker::unsubscribe(EventHandlerBase* subscriber)
-{
-    Lock<FastMutex> lock(mtxSubscribers);
-    for (auto it = subscribers.begin(); it != subscribers.end(); it++)
-    {
-        deleteSubscriber(it->second, subscriber);
-    }
-}
-
 void EventBroker::deleteSubscriber(vector<EventHandlerBase*>& subs,
                                    EventHandlerBase* subscriber)
 {
@@ -150,10 +155,4 @@ void EventBroker::deleteSubscriber(vector<EventHandlerBase*>& subs,
     }
 }
 
-void EventBroker::clearDelayedEvents()
-{
-    Lock<FastMutex> lock(mtxDelayedEvents);
-    delayedEvents.clear();
-}
-
 }  // namespace Boardcore
diff --git a/src/shared/events/EventBroker.h b/src/shared/events/EventBroker.h
index 27222a5a486524adefff64905e649689a422cda4..581ff8011164a47766542460c3959b9fcad1ec4f 100644
--- a/src/shared/events/EventBroker.h
+++ b/src/shared/events/EventBroker.h
@@ -62,8 +62,6 @@ class EventBroker : public Singleton<EventBroker>, public ActiveObject
 public:
     /**
      * Posts an event to the specified topic.
-     * @param ev
-     * @param topic
      */
     void post(const Event& ev, uint8_t topic);
 
@@ -150,14 +148,12 @@ public:
 
     /**
      * @brief Construct a new Event Broker object.
+     *
      * Public access required for testing purposes. Use the singleton interface
      * to access this class in production code.
-     *
      */
     EventBroker();
 
-    virtual ~EventBroker(){};
-
 private:
     /**
      * Private structure for holding a delayed event data in the list.
@@ -190,5 +186,3 @@ private:
 };
 
 }  // namespace Boardcore
-
-#define sEventBroker Boardcore::Singleton<Boardcore::EventBroker>::getInstance()
diff --git a/src/shared/events/FSM.h b/src/shared/events/FSM.h
index 0f32e4c973f5334264ee67cae30937dbff064fe4..f4ed570643450d16616eafcd84c575b02b6de859 100644
--- a/src/shared/events/FSM.h
+++ b/src/shared/events/FSM.h
@@ -63,8 +63,8 @@ FSM<T>::FSM(void (T::*initialState)(const Event&), unsigned int stacksize,
             miosix::Priority priority)
     : EventHandler(stacksize, priority)
 {
-    state             = initialState;
-    specialEvent.code = EV_ENTRY;
+    state        = initialState;
+    specialEvent = EV_ENTRY;
     postEvent(specialEvent);
 }
 
@@ -74,10 +74,10 @@ FSM<T>::~FSM(){};
 template <class T>
 void FSM<T>::transition(void (T::*nextState)(const Event&))
 {
-    specialEvent.code = EV_EXIT;
+    specialEvent = EV_EXIT;
     (static_cast<T*>(this)->*state)(specialEvent);
-    state             = nextState;
-    specialEvent.code = EV_ENTRY;
+    state        = nextState;
+    specialEvent = EV_ENTRY;
     (static_cast<T*>(this)->*state)(specialEvent);
 }
 
diff --git a/src/shared/events/utils/EventCounter.h b/src/shared/events/utils/EventCounter.h
index 27708720c00ab3a8987dfd98c0c70e6c9c568460..9f0914d26e17c0fca31960c706320deb5c5eebed 100644
--- a/src/shared/events/utils/EventCounter.h
+++ b/src/shared/events/utils/EventCounter.h
@@ -60,27 +60,22 @@ public:
     {
         Lock<FastMutex> l(mutex);
 
-        ++mapCounter[ev.code];
+        ++mapCounter[ev];
         ++totalCount;
 
-        lastEvent = ev.code;
+        lastEvent = ev;
     }
 
     /**
-     * @brief Returns the number of times a specific event has been received
+     * @brief Returns the number of times a specific event has been received.
      */
-    unsigned int getCount(const Event& ev) { return getCount(ev.code); }
-
-    /**
-     * @brief Returns the number of times a specific event has been received
-     */
-    unsigned int getCount(uint8_t evSig)
+    unsigned int getCount(const Event& ev)
     {
         Lock<FastMutex> l(mutex);
 
-        if (mapCounter.count(evSig) == 1)
+        if (mapCounter.count(ev) == 1)
         {
-            return mapCounter.at(evSig);
+            return mapCounter.at(ev);
         }
 
         return 0;
@@ -92,7 +87,7 @@ public:
     unsigned int getTotalCount() { return totalCount; }
 
     /**
-     * @brief Returns the signature of the last event received (ev.code)
+     * @brief Returns the signature of the last event received (ev)
      */
     uint8_t getLastEvent() { return lastEvent; }
 
diff --git a/src/shared/events/utils/EventInjector.h b/src/shared/events/utils/EventInjector.h
index d57c820f137c6385fb6ce75bbb7ede6a1a89692d..c9df410e213085153a1e6146e5b123d2fd89dcbd 100644
--- a/src/shared/events/utils/EventInjector.h
+++ b/src/shared/events/utils/EventInjector.h
@@ -57,7 +57,7 @@ protected:
             getline(cin, temp);
             stringstream(temp) >> ev >> topic;
 
-            sEventBroker.post({(uint8_t)ev}, topic);
+            EventBroker::getInstance().post({(uint8_t)ev}, topic);
         }
     }
 };
diff --git a/src/shared/events/utils/EventSniffer.h b/src/shared/events/utils/EventSniffer.h
index 19dff058ca32d00d0d6edf2c0b1755f62e317b69..67785774247ec3f5e6c17f5b2194365766d4e685 100644
--- a/src/shared/events/utils/EventSniffer.h
+++ b/src/shared/events/utils/EventSniffer.h
@@ -94,10 +94,7 @@ private:
             parent.broker.subscribe(this, topic);
         }
 
-        void postEvent(const Event& ev)
-        {
-            parent.onEventReceived(ev.code, topic);
-        }
+        void postEvent(const Event& ev) { parent.onEventReceived(ev, topic); }
 
         ~Sniffer() { parent.broker.unsubscribe(this); }
 
diff --git a/src/shared/logger/Deserializer.h b/src/shared/logger/Deserializer.h
index 63bdfbe53bec423e99ff24544062da452b0adba7..189582f0a17fcd6731236ca004dd3909e9b1d362 100644
--- a/src/shared/logger/Deserializer.h
+++ b/src/shared/logger/Deserializer.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2015-2018 Skyward Experimental Rocketry
- * Author: Luca Erbetta
+/* Copyright (c) 2015-2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -27,6 +27,7 @@
 
 #include <cstdio>
 #include <fstream>
+#include <iostream>
 #include <limits>
 #include <ostream>
 #include <string>
@@ -46,156 +47,189 @@ typedef std::numeric_limits<float> flt;
 // linter off
 
 /**
- * Class used to deserialize log files created using fedetft's logger.
+ * @brief Class used to deserialize the binary logs created using fedetft's
+ * logger into csv files.
  */
 class Deserializer
 {
 public:
-    Deserializer(std::string logfile, std::string prefix = "")
-        : prefix(prefix), logFile(logfile),
-          logFileWithExt(prefix + logFile + ".dat")
-    {
-    }
+    /**
+     * @brief Initializes the deserializer with the filename provided.
+     */
+    Deserializer(std::string fileName);
 
-    ~Deserializer()
-    {
-        if (!closed)
-        {
-            for (auto it = fileStreams.begin(); it != fileStreams.end(); it++)
-            {
-                (*it)->close();
-                delete *it;
-            }
-        }
-    }
+    ~Deserializer();
 
     /**
-     * Register a type to be deserialized, and the associated print function
+     * @brief Register a type to be deserialized.
+     *
+     * Node: The object type must provide a static header function and a print
+     * function with the following prototypes:
+     * static std::string header()
+     * void print(std::ostream& os) const
      *
-     * @param t the object to be deserialized
-     * @param fncPrint function that prints the deserialized data on the
-     * provided output stream.
+     * @tparam T The object type to be deserialized.
      */
     template <typename T>
-    bool registerType(std::function<void(T& t, std::ostream& os)> fncPrint,
-                      std::string header = "")
-    {
-        if (closed)
-        {
-            printf("Error: Deserializer is closed.\n");
-            return false;
-        }
+    void registerType();
+
+    /**
+     * @brief Deserializes the provided file.
+     *
+     * @return Whether the deserialization was successful.
+     */
+    bool deserialize();
+
+    /**
+     * @brief Closes all the openend files.
+     */
+    void close();
+
+private:
+    /**
+     * @brief Function to print a data element into its csv file.
+     *
+     * @tparam T Type of the object.
+     * @param t Object to be printed in the file.
+     * @param path Path where to save the file.
+     * @param prefix Prefix added to the file.
+     */
+    template <typename T>
+    void printType(T& t, std::string path = "", std::string prefix = "");
 
-        char cFilename[128];
-        sprintf(cFilename, "%s%s_%s.csv", prefix.c_str(), logFile.c_str(),
-                typeid(T).name());
+    bool closed = false;
 
-        std::string filename(cFilename);
+    std::map<std::string, std::ofstream*> fileStreams;
+    tscpp::TypePoolStream tps;
+
+    std::string logFilename;
+    std::string logFilenameWithoutExtension;
+    std::string logFolderPath;
+};
 
-        std::ofstream* stream = new std::ofstream();
+Deserializer::Deserializer(std::string logFilename) : logFilename(logFilename)
+{
+    // Prepare the folder path
+    logFilenameWithoutExtension =
+        logFilename.substr(0, logFilename.find_last_of("."));
+    logFolderPath = logFilenameWithoutExtension + "/";
+}
+
+Deserializer::~Deserializer() { close(); }
+
+template <typename T>
+void Deserializer::registerType()
+{
+    std::function<void(T & t)> callback =
+        std::bind(&Deserializer::printType<T>, this, std::placeholders::_1,
+                  logFolderPath, logFilenameWithoutExtension);
+
+    tps.registerType<T>(callback);
+}
+
+template <typename T>
+void Deserializer::printType(T& t, std::string path, std::string prefix)
+{
+    std::string demangledTypeName = tscpp::demangle(typeid(T).name());
+    static std::ofstream* stream;
+
+    try
+    {
+        stream = fileStreams.at(demangledTypeName);
+    }
+    // If not already initialize, open the file and write the header
+    catch (std::out_of_range e)
+    {
+        stream               = new std::ofstream();
+        std::string filename = path + prefix + "_" + demangledTypeName + ".csv";
+        std::cout << "Creating file " + filename << std::endl;
         stream->open(filename);
 
         if (!stream->is_open())
         {
             printf("Error opening file %s.\n", filename.c_str());
             perror("Error is:");
-            delete stream;
-            return false;
+            return;
         }
 
-        fileStreams.push_back(stream);
-        stream->precision(flt::max_digits10);  // Set stream precision to
-                                               // maximum float precision
-        // Print the header
-        if (header.length() > 0)
-        {
-            *stream << header;
-        }
+        // Print the header in the file
+        *stream << T::header();
 
-        using namespace std::placeholders;  // for _1
+        // Add the file to the vector such that it will be closed
+        fileStreams.emplace(demangledTypeName, stream);
+    }
 
-        std::function<void(T & t)> callback =
-            std::bind(fncPrint, _1, std::ref(*stream));
+    // Print data into the file if it is open
+    if (stream->is_open())
+        t.print(std::ref(*stream));
+}
 
-        tps.registerType<T>(callback);
+bool Deserializer::deserialize()
+{
+    if (closed)
+        return false;
 
-        return true;
-    }
+    // Create the folder
+    mkdir(logFolderPath.c_str(), 0777);
 
-    /**
-     * @brief Deserializes the provided file.
-     *
-     * @return Wheter the deserialization was successful.
-     */
-    bool deserialize()
+    // Move the log file into the folder
+    if (rename(logFilename.c_str(), (logFolderPath + logFilename).c_str()))
     {
-        if (closed)
-        {
-            return false;
-        }
+        std::cout << logFilename + " does not exists." << std::endl;
+        return false;
+    }
 
-        bool success = true;
-        std::string unknownTypeName;
+    // Open the log file
+    std::ifstream file(logFolderPath + logFilename);
 
-        struct stat st;
-        if (stat(logFileWithExt.c_str(), &st) != 0)
+    // Check if the file exists
+    if (!file)
+    {
+        std::cout << logFolderPath + logFilename + " does not exists."
+                  << std::endl;
+        return false;
+    }
+
+    tscpp::UnknownInputArchive inputArchive(file, tps);
+    while (true)
+    {
+        try
         {
-            printf("File %s does not exists.\n", logFileWithExt.c_str());
-            return false;
+            inputArchive.unserialize();
         }
-
-        std::ifstream file(logFileWithExt);
-        // file.open;
-        tscpp::UnknownInputArchive ia(file, tps);
-        int i = 0;
-        while (success)
+        catch (tscpp::TscppException& ex)
         {
-            try
+            // Reached end of file
+            if (strcmp(ex.what(), "eof") == 0)
             {
-                ia.unserialize();
+                return true;
             }
-            catch (tscpp::TscppException& ex)
+            // Unknown type found
+            else if (strcmp(ex.what(), "unknown type") == 0)
             {
-                // Reached end of file
-                if (strcmp(ex.what(), "eof") == 0)
-                {
-                    break;
-                }
-                else if (strcmp(ex.what(), "unknown type") == 0)
-                {
-                    unknownTypeName = ex.name();
-                    success         = false;
-                    printf("Unknown type found: %s\n", unknownTypeName.c_str());
-                    break;
-                }
+                std::string unknownTypeName = ex.name();
+                std::cout << "Unknown type found: " << unknownTypeName
+                          << std::endl;
+                return false;
             }
         }
-        file.close();
-        return success;
     }
 
-    void close()
+    file.close();
+    return true;
+}
+
+void Deserializer::close()
+{
+    if (!closed)
     {
-        if (!closed)
+        closed = true;
+        for (auto it = fileStreams.begin(); it != fileStreams.end(); it++)
         {
-            closed = true;
-            for (auto it = fileStreams.begin(); it != fileStreams.end(); it++)
-            {
-                (*it)->close();
-                delete *it;
-            }
+            it->second->close();
+            delete it->second;
         }
     }
-
-private:
-    bool closed = false;
-
-    std::vector<std::ofstream*> fileStreams;
-    tscpp::TypePoolStream tps;
-
-    std::string prefix;
-    std::string logFile;
-    std::string logFileWithExt;
-};
+}
 
 }  // namespace Boardcore
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
new file mode 100644
index 0000000000000000000000000000000000000000..6623fbf11bee706926ebb76ddb2c3d2c66aca122
--- /dev/null
+++ b/src/shared/logger/LogTypes.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2015-2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <actuators/Servo/ServoData.h>
+#include <algorithms/NAS/NASState.h>
+#include <diagnostic/PrintLoggerData.h>
+#include <drivers/adc/InternalADCData.h>
+#include <events/EventData.h>
+#include <logger/Deserializer.h>
+#include <logger/LoggerStats.h>
+#include <radio/MavlinkDriver/MavlinkStatus.h>
+#include <radio/Xbee/XbeeStatus.h>
+#include <scheduler/TaskSchedulerData.h>
+#include <sensors/ADS1118/ADS1118Data.h>
+#include <sensors/ADS131M04/ADS131M04Data.h>
+#include <sensors/BME280/BME280Data.h>
+#include <sensors/BMP280/BMP280Data.h>
+#include <sensors/BMX160/BMX160Data.h>
+#include <sensors/BMX160/BMX160WithCorrectionData.h>
+#include <sensors/HX711/HX711Data.h>
+#include <sensors/L3GD20/L3GD20Data.h>
+#include <sensors/LIS3DSH/LIS3DSHData.h>
+#include <sensors/LIS3MDL/LIS3MDLData.h>
+#include <sensors/MBLoadCell/MBLoadCellData.h>
+#include <sensors/MPU9250/MPU9250Data.h>
+#include <sensors/MS5803/MS5803Data.h>
+#include <sensors/SensorData.h>
+#include <sensors/UBXGPS/UBXGPSData.h>
+#include <sensors/UbloxGPS/UbloxGPSData.h>
+#include <sensors/VN100/VN100Data.h>
+#include <sensors/analog/BatteryVoltageSensorData.h>
+#include <sensors/analog/CurrentSensorData.h>
+#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h>
+#include <sensors/analog/pressure/honeywell/HSCMAND015PAData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN030PAData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN160KAData.h>
+#include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h>
+#include <sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h>
+
+#include <fstream>
+#include <iostream>
+
+/**
+ * @brief This file includes all the types the logdecoder script will decode.
+ *
+ * All logged classes inside Boardcore should be reported here.
+ */
+
+namespace Boardcore
+{
+
+namespace LogTypes
+{
+
+void registerTypes(Deserializer& ds)
+{
+    ds.registerType<ServoData>();
+    ds.registerType<NASState>();
+    ds.registerType<LoggingString>();
+    ds.registerType<InternalADCData>();
+    ds.registerType<EventData>();
+    ds.registerType<LoggerStats>();
+    ds.registerType<MavlinkStatus>();
+    ds.registerType<Xbee::XbeeStatus>();
+    ds.registerType<TaskStatsResult>();
+    ds.registerType<ADS1118Data>();
+    ds.registerType<ADS131M04Data>();
+    ds.registerType<BME280Data>();
+    ds.registerType<BMP280Data>();
+    ds.registerType<BMX160Data>();
+    ds.registerType<BMX160WithCorrectionData>();
+    ds.registerType<HX711Data>();
+    ds.registerType<L3GD20Data>();
+    ds.registerType<LIS3DSHData>();
+    ds.registerType<LIS3MDLData>();
+    ds.registerType<MBLoadCellData>();
+    ds.registerType<MPU9250Data>();
+    ds.registerType<MS5803Data>();
+    ds.registerType<TemperatureData>();
+    ds.registerType<UbloxGPSData>();
+    ds.registerType<UBXGPSData>();
+    ds.registerType<VN100Data>();
+    ds.registerType<BatteryVoltageSensorData>();
+    ds.registerType<CurrentSensorData>();
+    ds.registerType<MPXHZ6130AData>();
+    ds.registerType<HSCMAND015PAData>();
+    ds.registerType<HSCMRNN030PAData>();
+    ds.registerType<HSCMRNN160KAData>();
+    ds.registerType<SSCDANN030PAAData>();
+    ds.registerType<SSCDRRN015PDAData>();
+}
+
+}  // namespace LogTypes
+
+}  // namespace Boardcore
diff --git a/src/shared/logger/Logger.cpp b/src/shared/logger/Logger.cpp
index 2709fbcc2f7d3a6e9bf860f180f7036200b4a433..31876f6014c64a9e77bae874c4b26fcd7442725b 100644
--- a/src/shared/logger/Logger.cpp
+++ b/src/shared/logger/Logger.cpp
@@ -42,10 +42,10 @@ using namespace miosix;
 namespace Boardcore
 {
 
-int Logger::start()
+bool Logger::start()
 {
     if (started)
-        return fileNumber;
+        return true;
 
     // Find the proper log filename base on the current files on the disk
     string filename;
@@ -68,13 +68,13 @@ int Logger::start()
     {
         fileNumber = -1;
         TRACE("Error opening %s file\n", filename.c_str());
-        throw runtime_error("Error opening log file");
+        return false;
     }
     setbuf(file, NULL);  // Disable buffering for the file stream
 
-    // The boring part, start threads one by one and if they fail, undo
+    // The boring part, start threads one by one and if they fail, undo.
     // Perhaps excessive defensive programming as thread creation failure is
-    // highly unlikely (only if ram is full)
+    // highly unlikely (only if ram is full).
 
     packTh = Thread::create(packThreadLauncher, skywardStack(16 * 1024), 1,
                             this, Thread::JOINABLE);
@@ -82,7 +82,7 @@ int Logger::start()
     {
         fclose(file);
         TRACE("Error creating pack thread\n");
-        throw runtime_error("Error creating pack thread");
+        return false;
     }
 
     writeTh = Thread::create(writeThreadLauncher, skywardStack(16 * 1024), 1,
@@ -101,13 +101,13 @@ int Logger::start()
         fullBufferList.pop();  // Remove nullptr
         fclose(file);
         TRACE("Error creating write thread\n");
-        throw runtime_error("Error creating write thread");
+        return false;
     }
 
     started         = true;
     stats.logNumber = fileNumber;
 
-    return fileNumber;
+    return true;
 }
 
 void Logger::stop()
@@ -301,7 +301,6 @@ LoggerResult Logger::logImpl(const char* name, const void* data,
 {
     if (started == false)
     {
-        TRACE("Attempting to log %s but the Logger is not started!\n", name);
         stats.droppedSamples++;
 
         // Signal that we are trying to write to a closed log
diff --git a/src/shared/logger/Logger.h b/src/shared/logger/Logger.h
index 9679b837dea71c0d03de3f1bdb79719105c09278..f1ad95f7b077151a555348400c4ff10d82aa98ce 100644
--- a/src/shared/logger/Logger.h
+++ b/src/shared/logger/Logger.h
@@ -59,15 +59,17 @@ public:
     /**
      * @brief Call this function to start the logger.
      *
-     * When this function returns, the logger is started, and subsequent calls
-     * to log will actually log the data.
+     * The function tryies to start the logger. It first opens the log file and
+     * then create the pack and write threads. If it fails on one of this
+     * operation, the logger is not started.
+     *
+     * Use getCurrentLogNumber to retrieve the log file number.
      *
      * Blocking call. May take a long time.
      *
-     * \throws runtime_error if the log could not be opened.
-     * \return log number.
+     * \return true if the logger was started correctly.
      */
-    int start();
+    bool start();
 
     /**
      * @brief Call this function to stop the logger.
diff --git a/src/shared/math/SkyQuaternion.h b/src/shared/math/SkyQuaternion.h
deleted file mode 100644
index 24b424bc85b489f6c9df96ad3fc5f8de5faaa7e1..0000000000000000000000000000000000000000
--- a/src/shared/math/SkyQuaternion.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Marco Cella
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <Eigen/Dense>
-
-namespace Boardcore
-{
-
-/**
- * @brief Class for managing quaternions.
- *        Convention used: qx, qy, qz, qw (scalar element as last element)
- */
-class SkyQuaternion
-{
-
-public:
-    SkyQuaternion();
-
-    /**
-     * @brief Transform a vector of euler angles ZYX to quaternion.
-     *
-     * @param degeul the vector of euler angles to be transformed [deg]
-     *
-     * @return transformed quaternion
-     */
-    Eigen::Vector4f eul2quat(Eigen::Vector3f degeul);
-
-    /**
-     * @brief Transform a quaternion to a vector of euler angles.
-     *
-     * @param quat the quaternion to be transformed
-     *
-     * @return transformed vector of euler angles [deg]
-     */
-    Eigen::Vector3f quat2eul(Eigen::Vector4f quat);
-
-    /**
-     * @brief Transform a rotation matrix to a quaternion.
-     *
-     * @param R the rotation matrix to be transformed (3x3)
-     *
-     * @return transformed quaternion
-     */
-    Eigen::Vector4f rotm2quat(Eigen::Matrix3f R);
-
-    /**
-     * @brief Normalize a quaternion.
-     *
-     * @param quat the quaternion to be normalized
-     *
-     * @return boolean indicating if the operation succeded or not
-     */
-    bool quatnormalize(Eigen::Vector4f& quat);
-
-    /**
-     * @brief Compute the product of two quaternions.
-     *
-     * @param q1 the first factor
-     * @param q2 the second factor
-     *
-     * @return the resulting quaternions product
-     */
-    Eigen::Vector4f quatProd(const Eigen::Vector4f q1,
-                             const Eigen::Vector4f q2);
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg(
     const mavlink_message_t& msg)
 {
     // Convert mavlink message to char array
-    uint8_t msgtempBuf[PktLength];
+    // Use fixed buffer size to avoid overflows
+    uint8_t msgtempBuf[256];
     int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg);
 
     // Append message to the queue
@@ -347,11 +348,12 @@ void MavlinkDriver<PktLength, OutQueueSize>::runSender()
             // Get the first packet in the queue, without removing it
             pkt = outQueue.get();
 
-            uint64_t age = (uint64_t)miosix::getTick() - pkt.timestamp();
             // If the packet is ready or too old, send it
+            uint64_t age = TimestampTimer::getInstance().getTimestamp() -
+                           pkt.getTimestamp();
             if (pkt.isReady() || age >= outBufferMaxAge)
             {
-                outQueue.pop();  // remove from queue
+                outQueue.pop();  //  Remove the packet from queue
 
                 LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})",
                           pkt.size(), age);
diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp
index 52a91f3ba06520aa4b18974468319a22256b6774..bed1ae549d6daf57f3b77e8f6f0787c3efa9b5d9 100644
--- a/src/shared/radio/SX1278/SX1278.cpp
+++ b/src/shared/radio/SX1278/SX1278.cpp
@@ -31,6 +31,8 @@ namespace Boardcore
 
 using namespace SX1278Defs;
 
+long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; }
+
 // Default values for registers
 constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK |
                                         RegOpMode::MODULATION_TYPE_FSK |
@@ -247,10 +249,14 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
 
 bool SX1278::send(uint8_t *pkt, size_t len)
 {
-    // Packets longer than 255 are not supported
-    if (len > 255)
+    // Packets longer than FIFO_LEN (-1 for the len byte) are not supported
+    if (len > SX1278Defs::FIFO_LEN - 1)
         return false;
 
+    // This shouldn't be needed, but for some reason the device "lies" about
+    // being ready, so lock up if we are going too fast
+    rateLimitTx();
+
     bus_mgr.lock(SX1278BusManager::Mode::MODE_TX);
 
     // Wait for TX ready
@@ -267,11 +273,24 @@ bool SX1278::send(uint8_t *pkt, size_t len)
     // Wait for packet sent
     bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT);
     bus_mgr.unlock();
+
+    last_tx = now();
     return true;
 }
 
 void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); }
 
+void SX1278::rateLimitTx()
+{
+    const long long RATE_LIMIT = 2;
+
+    long long delta = now() - last_tx;
+    if (delta <= RATE_LIMIT)
+    {
+        miosix::Thread::sleep(RATE_LIMIT - delta);
+    }
+}
+
 uint8_t SX1278::getVersion()
 {
     SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
diff --git a/src/shared/radio/SX1278/SX1278.h b/src/shared/radio/SX1278/SX1278.h
index 6fb4c355d36f81331e7299e6a291e58c737e4156..3d89a9f9494a95ddcae4871f84a8c21132571066 100644
--- a/src/shared/radio/SX1278/SX1278.h
+++ b/src/shared/radio/SX1278/SX1278.h
@@ -228,6 +228,8 @@ public:
 public:
     using Mode = SX1278BusManager::Mode;
 
+    void rateLimitTx();
+
     void setBitrate(int bitrate);
     void setFreqDev(int freq_dev);
     void setFreqRF(int freq_rf);
@@ -240,6 +242,7 @@ public:
 
     void waitForIrq(uint16_t mask);
 
+    long long last_tx = 0;
     SX1278BusManager bus_mgr;
     PrintLogger logger = Logging::getLogger("sx1278");
 };
diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h
index cd50d09a8c3294659f1c1186f2ad1fd0512937c6..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644
--- a/src/shared/radio/SX1278/SX1278Defs.h
+++ b/src/shared/radio/SX1278/SX1278Defs.h
@@ -33,6 +33,11 @@ namespace Boardcore
 namespace SX1278Defs
 {
 
+/**
+ * @brief Length of the internal FIFO
+ */
+constexpr int FIFO_LEN = 64;
+
 /**
  * @brief Main oscillator frequency (Hz)
  */
diff --git a/src/shared/radio/Xbee/Xbee.h b/src/shared/radio/Xbee/Xbee.h
index ea748a68ff96aee0b6a58a78a4c9b447a3a3c41d..abb1be5015ba548258ab5207dd6abccff0bbd00a 100644
--- a/src/shared/radio/Xbee/Xbee.h
+++ b/src/shared/radio/Xbee/Xbee.h
@@ -38,7 +38,7 @@ using miosix::FastMutex;
 #ifndef USE_MOCK_PERIPHERALS
 using GpioType = miosix::GpioPin;
 #else
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
 using GpioType = MockGpioPin;
 #endif
 
diff --git a/src/shared/radio/Xbee/XbeeStatus.h b/src/shared/radio/Xbee/XbeeStatus.h
index 05f25880a39fa721c3453a6cce5597fdb2679925..ae0381dd915392ee8aa35e2a95f94a6a6d2baed2 100644
--- a/src/shared/radio/Xbee/XbeeStatus.h
+++ b/src/shared/radio/Xbee/XbeeStatus.h
@@ -22,7 +22,7 @@
 
 #pragma once
 
-#include <math/Stats.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdint>
 #include <cstdio>
diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index 26dc700a04091a73c7d9d9c33a38db9f3a4612d6..0518b34d9e8b23c4a65efc67804e1b1edbd87125 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -40,14 +40,6 @@ TaskScheduler::TaskScheduler()
 bool TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
                             Policy policy, int64_t startTick)
 {
-    // Perion must be grater than zero!
-    if (period <= 0 && policy != Policy::ONE_SHOT)
-    {
-        LOG_ERR(logger,
-                "Trying to add a task non one shot with an invalid period");
-        return false;
-    }
-
     Lock<FastMutex> lock(mutex);
 
     // Register the task into the map
@@ -69,6 +61,18 @@ bool TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
     return result.second;
 }
 
+bool TaskScheduler::addTask(function_t function, uint32_t period, Policy policy,
+                            int64_t startTick)
+{
+    uint8_t id = 1;
+
+    auto it = tasks.cbegin(), end = tasks.cend();
+    for (; it != end && id == it->first; ++it, ++id)
+        ;
+
+    return addTask(function, period, id, policy, startTick);
+}
+
 bool TaskScheduler::removeTask(uint8_t id)
 {
     Lock<FastMutex> lock(mutex);
@@ -99,6 +103,8 @@ bool TaskScheduler::removeTask(uint8_t id)
 
 bool TaskScheduler::start()
 {
+    // This check is necessary to prevent task normalization if the scheduler is
+    // already stopped
     if (running)
         return false;
 
@@ -128,7 +134,7 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats()
     return result;
 }
 
-void TaskScheduler ::normalizeTasks()
+void TaskScheduler::normalizeTasks()
 {
     int64_t currentTick = getTick();
 
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index 9a3ba6fe09c0940c59c20dcf643a2e5d282534c8..6eb0664158761b6cbc2b1eae124244429d890d94 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -25,7 +25,7 @@
 #include <ActiveObject.h>
 #include <Singleton.h>
 #include <diagnostic/PrintLogger.h>
-#include <math/Stats.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdint>
 #include <list>
@@ -110,6 +110,25 @@ public:
                  Policy policy     = Policy::SKIP,
                  int64_t startTick = miosix::getTick());
 
+    /**
+     * @brief Add a task function to the scheduler with an auto generated id.
+     *
+     * Note that each task has it's own unique ID, even one shot tasks!
+     * Therefore, if a task already exists with the same id, the function will
+     * fail and return false.
+     *
+     * For one shot tasks, the period is useless and not used.
+     *
+     * @param function Function to be called periodically.
+     * @param period Inter call period.
+     * @param policy Task policy, default is SKIP.
+     * @param startTick First activation time, useful for synchronizing tasks.
+     * @return true if the task was added successfully.
+     */
+    bool addTask(function_t function, uint32_t period,
+                 Policy policy     = Policy::SKIP,
+                 int64_t startTick = miosix::getTick());
+
     /**
      * @brief Removes the task identified by the given id if it exists.
      *
diff --git a/src/shared/scheduler/TaskSchedulerData.h b/src/shared/scheduler/TaskSchedulerData.h
index 35b04bbddc1627f8e6477ce6ebc1e44bdd13df4a..0cbe3865763d3d144b304ce2af5e26ea62dcb070 100644
--- a/src/shared/scheduler/TaskSchedulerData.h
+++ b/src/shared/scheduler/TaskSchedulerData.h
@@ -22,7 +22,7 @@
 
 #pragma once
 
-#include <math/Stats.h>
+#include <utils/Stats/Stats.h>
 
 #include <cstdint>
 #include <ostream>
diff --git a/src/shared/sensors/ADS1118/ADS1118.cpp b/src/shared/sensors/ADS1118/ADS1118.cpp
index 93724f64a2177cfdb5dd801c500deaee0d427dc8..b49ceb3c61a1da619856134ae200177a034ffb4d 100644
--- a/src/shared/sensors/ADS1118/ADS1118.cpp
+++ b/src/shared/sensors/ADS1118/ADS1118.cpp
@@ -236,7 +236,7 @@ void ADS1118::readChannel(int8_t nextChannel, int8_t prevChannel)
     }
 
     // Convert and save the value if last written configuration is valid
-    if (prevChannel >= 0)
+    if (prevChannel > INVALID_CHANNEL && prevChannel < NUM_OF_CHANNELS)
     {
         int16_t rawValue = swapBytes16(transferData);
 
diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp
index 53f2b2160f01eaee81a6151f4d4fd931fd4ef362..c8fd6e5400f5209f54a43a2897d990a0505d585a 100644
--- a/src/shared/sensors/BME280/BME280.cpp
+++ b/src/shared/sensors/BME280/BME280.cpp
@@ -66,21 +66,47 @@ bool BME280::init()
 
     loadCompensationParameters();
 
-    // Read once the temperature to compute fineTemperature
-    setConfiguration(BME280_CONFIG_TEMP_SINGLE);
-    miosix::Thread::sleep(
-        calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
-    readTemperature();
+    // Set the configuration 10 times to be sure
+    for (int i = 0; i < 10; i++)
+    {
+        // Read once the temperature to compute fineTemperature
+        setConfiguration(BME280_CONFIG_TEMP_SINGLE);
+        miosix::Thread::sleep(
+            calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
+        readTemperature();
 
-    setConfiguration();
+        setConfiguration();
+    }
+
+    // Set a sleep time to allow the sensor to change internally the data
+    miosix::Thread::sleep(100);
 
-    BME280Config readBackConfig = readConfiguration();
+    // I create the config state which represents the logic or of all the
+    // 5 readConfiguration controls (We perform 5 checks to avoid that the
+    // sensor is busy implicating in wrong responses)
+    bool readConfigResult = false;
+    BME280Config readBackConfig;
+
+    for (int i = 0; i < 10; i++)
+    {
+        readBackConfig = readConfiguration();
+        // Check if the configration on the device matches ours
+        if (config.bytes.ctrlHumidity == readBackConfig.bytes.ctrlHumidity &&
+            config.bytes.ctrlPressureAndTemperature ==
+                readBackConfig.bytes.ctrlPressureAndTemperature &&
+            config.bytes.config == readBackConfig.bytes.config)
+        {
+            readConfigResult = true;
+            break;
+        }
+
+        // After the check i sleep 100 milliseconds
+        miosix::Thread::sleep(20);
+    }
 
-    // Check if the configration on the device matches ours
-    if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity ||
-        config.bytes.ctrlPressureAndTemperature !=
-            readBackConfig.bytes.ctrlPressureAndTemperature ||
-        config.bytes.config != readBackConfig.bytes.config)
+    //   If after the 5 iterations the sensor didn't report the configuration
+    //   set I can report the init error
+    if (!readConfigResult)
     {
         LOG_ERR(logger, "Device configuration incorrect, setup failed");
 
diff --git a/src/shared/sensors/BMX160/BMX160.cpp b/src/shared/sensors/BMX160/BMX160.cpp
index 594c6a1854e8f2ece95d032c431f96f46a5f8083..287f9b5231dc7a08d87240f595a291b75a13089e 100644
--- a/src/shared/sensors/BMX160/BMX160.cpp
+++ b/src/shared/sensors/BMX160/BMX160.cpp
@@ -23,6 +23,7 @@
 #include "BMX160.h"
 
 #include <assert.h>
+#include <utils/Constants.h>
 
 namespace Boardcore
 {
@@ -598,25 +599,24 @@ MagnetometerData BMX160::buildMagData(BMX160Defs::MagRaw data,
 AccelerometerData BMX160::buildAccData(BMX160Defs::AccRaw data,
                                        uint64_t timestamp)
 {
-    return AccelerometerData{timestamp, data.x * accSensibility * EARTH_GRAVITY,
-                             data.y * accSensibility * EARTH_GRAVITY,
-                             data.z * accSensibility * EARTH_GRAVITY};
+    using namespace Constants;
+
+    return AccelerometerData{timestamp, data.x * accSensibility * g,
+                             data.y * accSensibility * g,
+                             data.z * accSensibility * g};
 }
 
 GyroscopeData BMX160::buildGyrData(BMX160Defs::GyrRaw data, uint64_t timestamp)
 {
+    using namespace Constants;
+
     if (config.gyroscopeUnit == BMX160Config::GyroscopeMeasureUnit::DEG)
-    {
         return GyroscopeData{timestamp, data.x * gyrSensibility,
                              data.y * gyrSensibility, data.z * gyrSensibility};
-    }
     else
-    {
-        return GyroscopeData{timestamp,
-                             data.x * gyrSensibility * DEGREES_TO_RADIANS,
-                             data.y * gyrSensibility * DEGREES_TO_RADIANS,
-                             data.z * gyrSensibility * DEGREES_TO_RADIANS};
-    }
+        return GyroscopeData{timestamp, data.x * gyrSensibility * g,
+                             data.y * gyrSensibility * g,
+                             data.z * gyrSensibility * g};
 }
 
 const char* BMX160::debugErr(SPITransaction& spi)
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 273ed10aa2f13339362a90b95247784e091931e0..5347ad81cdd5cbbded71cbdea25b7033c350f333 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -45,13 +45,9 @@ struct BMX160Data : public AccelerometerData,
     static std::string header()
     {
         return "accelerationTimestamp,accelerationX,accelerationY,"
-               "accelerationZ,gyro_"
-               "timestamp,"
-               "angularVelocityX,"
-               "angularVelocityY,"
-               "angularVelocityZ,magneticFieldTimestamp,magneticFieldX,"
-               "magneticFieldY,"
-               "magneticFieldZ\n";
+               "accelerationZ,angularVelocityTimestamp,angularVelocityX,"
+               "angularVelocityY,angularVelocityZ,magneticFieldTimestamp,"
+               "magneticFieldX,magneticFieldY,magneticFieldZ\n";
     }
 
     void print(std::ostream& os) const
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h
index 627685ec1fec4a5c62dc6907cfe3a2178d40bd0f..fa139ef6bca95e2b6296a97be26185c050fedfd4 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.h
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h
@@ -24,6 +24,7 @@
 
 #include <diagnostic/PrintLogger.h>
 #include <sensors/BMX160/BMX160.h>
+#include <sensors/calibration/AxisOrientation.h>
 #include <sensors/calibration/BiasCalibration.h>
 #include <sensors/calibration/SixParameterCalibration.h>
 
diff --git a/src/shared/sensors/HX711/HX711.cpp b/src/shared/sensors/HX711/HX711.cpp
index b08b99495064c948fb05c1af2d48101d3fd6659d..8e3fa92694daa960e71db8a1463147e11bdd3e9d 100644
--- a/src/shared/sensors/HX711/HX711.cpp
+++ b/src/shared/sensors/HX711/HX711.cpp
@@ -63,19 +63,34 @@ HX711Data HX711::sampleImpl()
     sckPin.alternateFunction(sckAlternateFunction);
 
     if (sample & static_cast<int32_t>(0x800000))
-        sample |= 0xFF << 24;
+        sample |= static_cast<uint32_t>(0xFF) << 24;
 
     if (sample == static_cast<int32_t>(0xFFFFFFFF))
         return lastSample;
 
     return {TimestampTimer::getInstance().getTimestamp(),
-            (sample + offset) / scale};
+            static_cast<float>(sample - offset) * scale};
 }
 
+void HX711::computeScale(float value, float sample)
+{
+    // Convert the sample in raw measurement with current scale factor
+    sample = sample / scale;
+
+    // Update the scale such that the sample corresponds to the given value
+    this->scale = value / sample;
+}
+
+void HX711::computeScale(float value) { computeScale(value, lastSample.load); }
+
 void HX711::setScale(float scale) { this->scale = scale; }
 
-void HX711::setZero() { offset = -lastSample.weight * scale; }
+float HX711::getScale() { return scale; }
+
+void HX711::setOffset(float offset) { this->offset = offset / scale; }
+
+void HX711::updateOffset(float offset) { this->offset += offset / scale; }
 
-void HX711::setZero(float offset) { this->offset = offset * scale; }
+float HX711::getOffset() { return offset; }
 
-}  // namespace Boardcore
\ No newline at end of file
+}  // namespace Boardcore
diff --git a/src/shared/sensors/HX711/HX711.h b/src/shared/sensors/HX711/HX711.h
index 24d699ae5fef8857acb4c0152cd71792d1c8a783..4674ea16e84c255470bb80ec54aebb997d624665 100644
--- a/src/shared/sensors/HX711/HX711.h
+++ b/src/shared/sensors/HX711/HX711.h
@@ -65,23 +65,52 @@ public:
     bool selfTest() override;
 
     /**
-     * @brief Sets the scale value.
+     * @brief Calculates the scale value such that the load cell's output
+     * matches the given value.
      *
-     * This value is used to convert the raw data into weight.
+     * The value is used to compute the scale coefficient in this way:
+     * scale = value / (sample - offset)
+     *
+     * @param value Value that the load cell should read now.
+     * @param sample Sensor sample used to compute the scale.
+     */
+    void computeScale(float value, float sample);
+
+    /**
+     * @brief Same as computeScale but uses the last sample.
+     */
+    void computeScale(float value);
+
+    /**
+     * @brief Simply changes the scale.
+     *
+     * @param scale New scale value.
      */
     void setScale(float scale);
 
     /**
-     * @brief Sets the offset to zero the current measurement.
+     * @brief Returns the current scale.
      */
-    void setZero();
+    float getScale();
 
     /**
      * @brief Sets the offset to the given value.
      *
-     * @param offset Offset in Kg.
+     * @param offset Offset that will be removed from the measurement.
+     */
+    void setOffset(float offset);
+
+    /**
+     * @brief Updates the offset by adding it to the current offset.
+     *
+     * @param offset Offset that will be removed from the measurement.
+     */
+    void updateOffset(float offset);
+
+    /**
+     * @brief Return the current offset.
      */
-    void setZero(float offset);
+    float getOffset();
 
 private:
     HX711Data sampleImpl() override;
diff --git a/src/shared/sensors/HX711/HX711Data.h b/src/shared/sensors/HX711/HX711Data.h
index 6d4f4940229b86fd157f6b4b133ee6b7ec8c9d14..49e25db1942c56331a90e9bbe6cd29f21741607d 100644
--- a/src/shared/sensors/HX711/HX711Data.h
+++ b/src/shared/sensors/HX711/HX711Data.h
@@ -32,17 +32,17 @@ struct HX711Data : public LoadCellData
 {
     HX711Data() : LoadCellData{0, 0} {}
 
-    HX711Data(uint64_t weightTimestamp, float weight)
-        : LoadCellData{weightTimestamp, weight}
+    HX711Data(uint64_t loadTimestamp, float load)
+        : LoadCellData{loadTimestamp, load}
     {
     }
 
-    static std::string header() { return "weightTimestamp,weight\n"; }
+    static std::string header() { return "loadTimestamp,load\n"; }
 
     void print(std::ostream& os) const
     {
-        os << weightTimestamp << "," << weight << "\n";
+        os << loadTimestamp << "," << load << "\n";
     }
 };
 
-}  // namespace Boardcore
\ No newline at end of file
+}  // namespace Boardcore
diff --git a/src/shared/sensors/L3GD20/L3GD20.h b/src/shared/sensors/L3GD20/L3GD20.h
index bdd7c78fda3d8d82fffaddd95d0f3a915613be65..802b0ed2fefad0331afcd5da5598178984e4eadc 100644
--- a/src/shared/sensors/L3GD20/L3GD20.h
+++ b/src/shared/sensors/L3GD20/L3GD20.h
@@ -25,10 +25,10 @@
 
 #include <drivers/spi/SPIDriver.h>
 #include <drivers/timer/TimestampTimer.h>
-#include <math/Vec3.h>
 #include <miosix.h>
 #include <sensors/Sensor.h>
 
+#include <Eigen/Core>
 #include <array>
 
 #include "L3GD20Data.h"
@@ -220,9 +220,8 @@ public:
             int16_t y = buf[2] | buf[3] << 8;
             int16_t z = buf[4] | buf[5] << 8;
 
-            Vec3 rads   = toRadiansPerSecond(x, y, z);
-            lastFifo[0] = {lastSampleTimestamp, rads.getX(), rads.getY(),
-                           rads.getZ()};
+            Eigen::Vector3f rads = toRadiansPerSecond(x, y, z);
+            lastFifo[0] = {lastSampleTimestamp, rads(0), rads(1), rads(2)};
         }
 
         else  // FIFO is enabled
@@ -243,7 +242,7 @@ public:
             {
                 bool rmv;
                 // Check for duplicates: there seems to be a bug where the
-                // sensor occasionaly shifts out the same sample two times in a
+                // sensor occasionally shifts out the same sample two times in a
                 // row: discard one
                 if (i < fifoLevel - 1)
                 {
@@ -276,7 +275,7 @@ public:
                 // Ignoring possible duplicates, the timestamp of the ith sample
                 // in the fifo is:
                 // ts(i) = ts(fifoWatermark) + (i - fifoWatermark)*dt;
-                Vec3 rads =
+                Eigen::Vector3f rads =
                     toRadiansPerSecond(buf[i * 6] | buf[i * 6 + 1] << 8,
                                        buf[i * 6 + 2] | buf[i * 6 + 3] << 8,
                                        buf[i * 6 + 4] | buf[i * 6 + 5] << 8);
@@ -284,7 +283,7 @@ public:
                 lastFifo[i - duplicates] = L3GD20Data{
                     lastInterruptTimestamp +
                         ((int)i - (int)fifoWatermark - (int)duplicates) * dt,
-                    rads.getX(), rads.getY(), rads.getZ()};
+                    rads(0), rads(1), rads(2)};
             }
 
             lastFifoLevel = fifoLevel - duplicates;
@@ -294,9 +293,10 @@ public:
     }
 
 private:
-    Vec3 toRadiansPerSecond(int16_t x, int16_t y, int16_t z)
+    Eigen::Vector3f toRadiansPerSecond(int16_t x, int16_t y, int16_t z)
     {
-        return Vec3(x * sensitivity, y * sensitivity, z * sensitivity);
+        return Eigen::Vector3f(x * sensitivity, y * sensitivity,
+                               z * sensitivity);
     }
 
     static constexpr float SENSITIVITY_250  = 0.00875f;
diff --git a/src/shared/sensors/MAX31855/MAX31855.cpp b/src/shared/sensors/MAX31855/MAX31855.cpp
index 137cf2569ceb93d17e72fbfa07a38243daa0563f..3a723a6d8a7f46878fafd41162925a734fccf6ab 100644
--- a/src/shared/sensors/MAX31855/MAX31855.cpp
+++ b/src/shared/sensors/MAX31855/MAX31855.cpp
@@ -59,7 +59,7 @@ bool MAX31855::checkConnection()
     if ((sample[1] & 0x7) != 0)
     {
         lastError = SensorErrors::SELF_TEST_FAIL;
-        LOG_ERR(logger, "Self test failed, the termocouple is not connected");
+        LOG_ERR(logger, "Self test failed, the thermocouple is not connected");
         return false;
     }
 
diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
index 1874fff78cb19a8b22ff69cefb07e4c1462c9bb2..7f943cd5c2078d7d26c0d0b4ecf2db0433c67416 100644
--- a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
+++ b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
@@ -152,28 +152,28 @@ void MBLoadCell::printData()
 
     if (maxPrint)
     {
-        TRACE("NEW MAX %.2f (ts: %.3f [s])\n", maxWeight.weight,
-              maxWeight.weightTimestamp / 1000000.0);
+        TRACE("NEW MAX %.2f (ts: %.3f [s])\n", maxWeight.load,
+              maxWeight.loadTimestamp / 1000000.0);
         maxPrint = false;
     }
 
     if (minPrint)
     {
-        TRACE("NEW MIN %.2f (ts: %.3f [s])\n", minWeight.weight,
-              minWeight.weightTimestamp / 1000000.0);
+        TRACE("NEW MIN %.2f (ts: %.3f [s])\n", minWeight.load,
+              minWeight.loadTimestamp / 1000000.0);
         minPrint = false;
     }
 }
 
-Data MBLoadCell::getMaxWeight() { return maxWeight; }
+MBLoadCellData MBLoadCell::getMaxWeight() { return maxWeight; }
 
-Data MBLoadCell::getMinWeight() { return minWeight; }
+MBLoadCellData MBLoadCell::getMinWeight() { return minWeight; }
 
 bool MBLoadCell::selfTest() { return true; }
 
-Data MBLoadCell::sampleImpl()
+MBLoadCellData MBLoadCell::sampleImpl()
 {
-    Data value;
+    MBLoadCellData value;
     switch (settings.mode)
     {
         case LoadCellModes::CONT_MOD_T:
@@ -191,7 +191,7 @@ Data MBLoadCell::sampleImpl()
     }
 
     // Memorizing also the maximum gross weight registered
-    if (!maxWeight.valid || maxWeight.weight < value.weight)
+    if (!maxWeight.valid || maxWeight.load < value.load)
     {
         maxWeight = value;
         maxSetted = true;
@@ -203,7 +203,7 @@ Data MBLoadCell::sampleImpl()
     }
 
     // Memorizing also the minimum gross weight registered
-    if (!minWeight.valid || minWeight.weight > value.weight)
+    if (!minWeight.valid || minWeight.load > value.load)
     {
         minWeight = value;
         minSetted = true;
@@ -217,23 +217,23 @@ Data MBLoadCell::sampleImpl()
     return value;
 }
 
-Data MBLoadCell::sampleContModT()
+MBLoadCellData MBLoadCell::sampleContModT()
 {
     DataModT data;
     receive(&data);
 
-    return Data(atof(data.weight) / 10.0);
+    return MBLoadCellData(atof(data.weight) / 10.0);
 }
 
-Data MBLoadCell::sampleContModTd()
+MBLoadCellData MBLoadCell::sampleContModTd()
 {
     DataModTd data;
     receive(&data);
 
-    return Data(atof(data.weightT) / 10.0);
+    return MBLoadCellData(atof(data.weightT) / 10.0);
 }
 
-Data MBLoadCell::sampleAsciiModTd()
+MBLoadCellData MBLoadCell::sampleAsciiModTd()
 {
     DataAsciiRequest request;
 
@@ -262,7 +262,7 @@ Data MBLoadCell::sampleAsciiModTd()
     else
     {
         // Taking the value returned
-        return Data(stof(response.substr(3, 6)) / 10.0);
+        return MBLoadCellData(stof(response.substr(3, 6)) / 10.0);
     }
 }
 
diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.h b/src/shared/sensors/MBLoadCell/MBLoadCell.h
index aefe3c6808653ca2ca93abd0742feae5d8f23273..d8e57f5f65bdded510a9ad6830bed13d1c053106 100644
--- a/src/shared/sensors/MBLoadCell/MBLoadCell.h
+++ b/src/shared/sensors/MBLoadCell/MBLoadCell.h
@@ -47,7 +47,7 @@ namespace Boardcore
  * - ASCII-modTd: bidirectional mode that consists in sending a request and
  * receiving a response with the data requested or an error message
  */
-class MBLoadCell : public Sensor<Data>
+class MBLoadCell : public Sensor<MBLoadCellData>
 {
 public:
     /**
@@ -86,12 +86,12 @@ public:
     /**
      * @brief Returns a copy of the max weight detected.
      */
-    Data getMaxWeight();
+    MBLoadCellData getMaxWeight();
 
     /**
      * @brief Returns a copy of the min weight detected.
      */
-    Data getMinWeight();
+    MBLoadCellData getMinWeight();
 
     bool selfTest() override;
 
@@ -102,22 +102,22 @@ protected:
      *
      * @return The weight measured from the load cell.
      */
-    Data sampleImpl() override;
+    MBLoadCellData sampleImpl() override;
 
     /**
      * @brief Sampling in the "continuous Mod T" mode.
      */
-    Data sampleContModT(void);
+    MBLoadCellData sampleContModT(void);
 
     /**
      * @brief Sampling in the "continuous Mod Td" mode.
      */
-    Data sampleContModTd(void);
+    MBLoadCellData sampleContModTd(void);
 
     /**
      * @brief Sampling in the "ASCII Mod Td" mode.
      */
-    Data sampleAsciiModTd(void);
+    MBLoadCellData sampleAsciiModTd(void);
 
     /**
      * @brief Forges a request for the ascii mode.
@@ -158,8 +158,8 @@ protected:
 
 private:
     MBLoadCellSettings settings;  ///< Contains all the configuration
-    Data maxWeight;               ///< Maximum weight detected by the load cell
-    Data minWeight;               ///< Minimum weight detected by the load cell
+    MBLoadCellData maxWeight;     ///< Maximum weight detected by the load cell
+    MBLoadCellData minWeight;     ///< Minimum weight detected by the load cell
     bool maxSetted;
     bool maxPrint;
     bool minSetted;
diff --git a/src/shared/sensors/MBLoadCell/MBLoadCellData.h b/src/shared/sensors/MBLoadCell/MBLoadCellData.h
index c576fa20e15f691d11f8a060e6545f449c786310..3c9c12495040743ac5dc2bfd0a7647f62115ee94 100644
--- a/src/shared/sensors/MBLoadCell/MBLoadCellData.h
+++ b/src/shared/sensors/MBLoadCell/MBLoadCellData.h
@@ -22,9 +22,9 @@
 
 #pragma once
 
-#include <drivers/timer/TimestampTimer.h>
 #include <utils/Debug.h>
 
+#include <cstdio>
 #include <map>
 
 #include "sensors/SensorData.h"
@@ -87,24 +87,25 @@ enum ReturnsStates
  * @brief Structure that stores a data value, with his timestamp and his
  * validity.
  */
-struct Data : public LoadCellData
+struct MBLoadCellData : public LoadCellData
 {
     bool valid = false;
 
-    Data() : LoadCellData{0, 0.0}, valid(false) {}
+    MBLoadCellData() : LoadCellData{0, 0.0}, valid(false) {}
 
-    explicit Data(float data)
-        : LoadCellData{TimestampTimer::getInstance().getTimestamp(), data},
-          valid(true)
+    explicit MBLoadCellData(float data) : MBLoadCellData{0, data} {}
+
+    explicit MBLoadCellData(uint64_t loadTimestamp, float data)
+        : LoadCellData{loadTimestamp, data}, valid(true)
     {
     }
 
-    static std::string header() { return "weightTimestamp,weight\n"; }
+    static std::string header() { return "loadTimestamp,weight\n"; }
 
     void print(std::ostream& os) const
     {
         if (valid)
-            os << weightTimestamp / 1000000.0 << "," << weight << "\n";
+            os << loadTimestamp / 1000000.0 << "," << load << "\n";
     }
 };
 
@@ -115,10 +116,10 @@ struct MBLoadCellSettings
 {
     LoadCellModes mode;
     bool grossMode;
-    Data peakWeight;
-    Data setpoint1;
-    Data setpoint2;
-    Data setpoint3;
+    MBLoadCellData peakWeight;
+    MBLoadCellData setpoint1;
+    MBLoadCellData setpoint2;
+    MBLoadCellData setpoint3;
 
     /**
      * @brief Updates the correct value with the data passed. Also, memorizes
@@ -129,16 +130,16 @@ struct MBLoadCellSettings
         switch (val)
         {
             case PEAK_WEIGHT:
-                peakWeight = Data(data);
+                peakWeight = MBLoadCellData(data);
                 break;
             case GET_SETPOINT_1:
-                setpoint1 = Data(data);
+                setpoint1 = MBLoadCellData(data);
                 break;
             case GET_SETPOINT_2:
-                setpoint2 = Data(data);
+                setpoint2 = MBLoadCellData(data);
                 break;
             case GET_SETPOINT_3:
-                setpoint3 = Data(data);
+                setpoint3 = MBLoadCellData(data);
                 break;
             default:
                 break;
@@ -151,22 +152,22 @@ struct MBLoadCellSettings
     void print() const
     {
         /*if (netWeight.valid)
-            TRACE("Net Weight     : %f [Kg]\n", netWeight.data);
+            TRACE("Net Weight     : %f [Kg]\n", netWeight.load);
 
         if (grossWeight.valid)
-            TRACE("Gross Weight   : %f [Kg]\n", grossWeight.data);
+            TRACE("Gross Weight   : %f [Kg]\n", grossWeight.load);
         */
         if (peakWeight.valid)
-            TRACE("Peak Weight    : %f [Kg]\n", peakWeight.weight);
+            TRACE("Peak Weight    : %f [Kg]\n", peakWeight.load);
 
         if (setpoint1.valid)
-            TRACE("Setpoint 1     : %f [Kg]\n", setpoint1.weight);
+            TRACE("Setpoint 1     : %f [Kg]\n", setpoint1.load);
 
         if (setpoint2.valid)
-            TRACE("Setpoint 2     : %f [Kg]\n", setpoint2.weight);
+            TRACE("Setpoint 2     : %f [Kg]\n", setpoint2.load);
 
         if (setpoint3.valid)
-            TRACE("Setpoint 3     : %f [Kg]\n", setpoint3.weight);
+            TRACE("Setpoint 3     : %f [Kg]\n", setpoint3.load);
     }
 };
 
@@ -223,7 +224,7 @@ struct DataAsciiRequest
             checksum ^= str[i];
         }
 
-        itoa(checksum, ck, 16);
+        sprintf(ck, "%x", checksum);
     }
 
     /**
diff --git a/src/shared/sensors/MPU9250/MPU9250.cpp b/src/shared/sensors/MPU9250/MPU9250.cpp
index 4f0d1eaa1ed0e0849ef1fbd41080a0d32a12e5dd..0348e7afe5ae76c272fc2cbea109439f795dbcea 100644
--- a/src/shared/sensors/MPU9250/MPU9250.cpp
+++ b/src/shared/sensors/MPU9250/MPU9250.cpp
@@ -29,11 +29,12 @@
 namespace Boardcore
 {
 
-MPU9250::MPU9250(SPISlave spiSlave_, unsigned short samplingRate_,
-                 MPU9250GyroFSR gyroFsr_, MPU9250AccelFSR accelFsr_,
-                 SPI::ClockDivider highSpeedSpiClockDivider_)
-    : spiSlave(spiSlave_), samplingRate(samplingRate_), gyroFsr(gyroFsr_),
-      accelFsr(accelFsr_), highSpeedSpiClockDivider(highSpeedSpiClockDivider_)
+MPU9250::MPU9250(SPIBusInterface& bus, miosix::GpioPin cs, SPIBusConfig config,
+                 unsigned short samplingRate, MPU9250GyroFSR gyroFsr,
+                 MPU9250AccelFSR accelFsr,
+                 SPI::ClockDivider highSpeedSpiClockDivider)
+    : spiSlave(bus, cs, config), samplingRate(samplingRate), gyroFsr(gyroFsr),
+      accelFsr(accelFsr), highSpeedSpiClockDivider(highSpeedSpiClockDivider)
 {
 }
 
@@ -434,7 +435,7 @@ void MPU9250::writeSPIWithDelay(SPITransaction& transaction, uint8_t reg,
 float MPU9250::normalizeAcceleration(int16_t rawValue)
 {
     return static_cast<float>(rawValue) / 32768.0f *
-           ACCELERATION_FS_MAP[accelFsr >> 3] * EARTH_GRAVITY;
+           ACCELERATION_FS_MAP[accelFsr >> 3] * Constants::g;
 }
 
 // Page 33 of register map document
@@ -446,7 +447,7 @@ float MPU9250::normalizeTemperature(int16_t rawValue)
 float MPU9250::normalizeGyroscope(int16_t rawValue)
 {
     return static_cast<float>(rawValue) / 32768.0f *
-           GYROSCOPE_FS_MAP[gyroFsr >> 3] * DEGREES_TO_RADIANS;
+           GYROSCOPE_FS_MAP[gyroFsr >> 3] * Constants::DEGREES_TO_RADIANS;
 }
 
 float MPU9250::normalizeMagnetometer(int16_t rawValue, float adjustmentCoeff)
diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h
index e6cf27e7f66ab86346e95c59c6605f841098270c..039eda52f5177318bdea77a24679c5e31079e1cb 100644
--- a/src/shared/sensors/MPU9250/MPU9250.h
+++ b/src/shared/sensors/MPU9250/MPU9250.h
@@ -189,14 +189,16 @@ public:
     /**
      * @brief Instantiates the driver
      *
-     * @param highSpeedSpiClockDivider_ Clocl diver for 20MHz SPI communication
+     * @param highSpeedSpiClockDivider Clocl diver for 20MHz SPI communication
      * with the device
      */
     explicit MPU9250(
-        SPISlave spiSlave_, unsigned short samplingRate_ = 100,
-        MPU9250GyroFSR gyroFsr_                     = GYRO_FSR_250DPS,
-        MPU9250AccelFSR accelFsr_                   = ACCEL_FSR_2G,
-        SPI::ClockDivider highSpeedSpiClockDivider_ = SPI::ClockDivider::DIV_4);
+        SPIBusInterface& bus, miosix::GpioPin cs,
+        SPIBusConfig config                        = getDefaultSPIConfig(),
+        unsigned short samplingRate                = 100,
+        MPU9250GyroFSR gyroFsr                     = GYRO_FSR_250DPS,
+        MPU9250AccelFSR accelFsr                   = ACCEL_FSR_2G,
+        SPI::ClockDivider highSpeedSpiClockDivider = SPI::ClockDivider::DIV_4);
 
     /**
      * @brief Constructs the default config for SPI Bus.
diff --git a/src/shared/sensors/MS5803/MS5803.cpp b/src/shared/sensors/MS5803/MS5803.cpp
index 1a2735f642e15deb572aa2e0c8bcd7be29d67636..52eec1bb6c3f0c15e833b75eaea66205c26b56e2 100644
--- a/src/shared/sensors/MS5803/MS5803.cpp
+++ b/src/shared/sensors/MS5803/MS5803.cpp
@@ -28,15 +28,9 @@
 namespace Boardcore
 {
 
-MS5803::MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_)
-    : spiSlave(spiSlave_), temperatureDivider(temperatureDivider_)
-{
-}
-
-MS5803::MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_,
-               SPIBusConfig spiConfig_, uint16_t temperatureDivider_)
-    : spiSlave(spiBus_, cs_, spiConfig_),
-      temperatureDivider(temperatureDivider_)
+MS5803::MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs,
+               SPIBusConfig spiConfig, uint16_t temperatureDivider)
+    : spiSlave(spiBus, cs, spiConfig), temperatureDivider(temperatureDivider)
 {
 }
 
diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h
index 427cb78ca57671c6afda72e3c4e4c0941f106132..6065ebcbab862231cf4fcd37e12d857cb4119198 100644
--- a/src/shared/sensors/MS5803/MS5803.h
+++ b/src/shared/sensors/MS5803/MS5803.h
@@ -73,9 +73,8 @@ public:
 
     static constexpr uint8_t TIMEOUT = 5;
 
-    explicit MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_ = 1);
-    MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_,
-           SPIBusConfig spiConfig_, uint16_t temperatureDivider_ = 1);
+    MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs,
+           SPIBusConfig spiConfig = {}, uint16_t temperatureDivider = 1);
 
     bool init() override;
 
diff --git a/src/shared/sensors/MS5803/MS5803Data.h b/src/shared/sensors/MS5803/MS5803Data.h
index 596dcec13c84878696b79119a5be30b1be0f5a3a..10eb959bd39c60b15e4c1abac74986f517d7ce3d 100644
--- a/src/shared/sensors/MS5803/MS5803Data.h
+++ b/src/shared/sensors/MS5803/MS5803Data.h
@@ -55,7 +55,7 @@ struct MS5803Data : public PressureData, TemperatureData
 
     static std::string header()
     {
-        return "pressureTimestamp,press,temperatureTimestamp,temp\n";
+        return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n";
     }
 
     void print(std::ostream& os) const
diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h
index de72201bf50d2fc64cb19bdc744238cc8076aa4f..2a2a2ecb08270a9b4e107871e0480b4bb7b42f79 100644
--- a/src/shared/sensors/SensorData.h
+++ b/src/shared/sensors/SensorData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Luca Conterio
+/* Copyright (c) 2020-2022 Skyward Experimental Rocketry
+ * Authors: Luca Conterio, 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
@@ -56,8 +56,15 @@ struct TimestampData
 
 struct LoadCellData
 {
-    uint64_t weightTimestamp;
-    float weight;
+    uint64_t loadTimestamp;
+    float load;
+
+    static std::string header() { return "loadTimestamp,load\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << loadTimestamp << "," << load << "\n";
+    }
 };
 
 struct TemperatureData
diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp
index 39f775e97e642c8699797cbc15da24150fae855d..53ac4adc512f65aea68ca0e313ee6b51b3984870 100644
--- a/src/shared/sensors/SensorManager.cpp
+++ b/src/shared/sensors/SensorManager.cpp
@@ -50,7 +50,12 @@ SensorManager::~SensorManager()
         delete scheduler;
 }
 
-bool SensorManager::start() { return scheduler->start() && initResult; }
+bool SensorManager::start()
+{
+    if (customScheduler)
+        scheduler->start();
+    return initResult;
+}
 
 void SensorManager::stop() { scheduler->stop(); }
 
@@ -173,11 +178,9 @@ bool SensorManager::init(const SensorMap_t& sensorsMap)
             samplersMap[sensor] = newSampler;
 
             if (currentSamplerId == MAX_TASK_ID)
-            {
                 LOG_WARN(logger,
                          "Max task ID (255) reached in task scheduler, IDs "
                          "will start again from 0");
-            }
 
             currentSamplerId++;
         }
diff --git a/src/shared/sensors/SensorManager.h b/src/shared/sensors/SensorManager.h
index 4fb828a37ce87b05c59321dc78902a54a5a58fd3..3f6cd41900795f9ebcee5c4a26ca9b0891598177 100644
--- a/src/shared/sensors/SensorManager.h
+++ b/src/shared/sensors/SensorManager.h
@@ -59,8 +59,14 @@ public:
      */
     ~SensorManager();
 
+    /**
+     * @brief Starts the task scheduler.
+     */
     bool start();
 
+    /**
+     * @brief Starts the task scheduler.
+     */
     void stop();
 
     /**
@@ -95,7 +101,7 @@ public:
     const vector<TaskStatsResult> getSamplersStats();
 
 private:
-    SensorManager(const SensorManager&) = delete;
+    SensorManager(const SensorManager&)            = delete;
     SensorManager& operator=(const SensorManager&) = delete;
 
     /**
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce
--- /dev/null
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
@@ -0,0 +1,537 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, 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 "UbloxGPS.h"
+
+#include <diagnostic/StackLogger.h>
+#include <drivers/serial.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <fcntl.h>
+#include <filesystem/file_access.h>
+
+using namespace miosix;
+
+namespace Boardcore
+{
+
+UbloxGPS::UbloxGPS(int baudrate_, uint8_t sampleRate_, int serialPortNum_,
+                   const char* serialPortName_, int defaultBaudrate_)
+    : baudrate(baudrate_), sampleRate(sampleRate_),
+      serialPortNumber(serialPortNum_), serialPortName(serialPortName_),
+      defaultBaudrate(defaultBaudrate_)
+{
+    // Prepare the gps file path with the specified name
+    strcpy(gpsFilePath, "/dev/");
+    strcat(gpsFilePath, serialPortName);
+}
+
+bool UbloxGPS::init()
+{
+    // Change the baud rate from the default value
+    if (!serialCommuinicationSetup())
+    {
+        return false;
+    }
+
+    Thread::sleep(10);
+
+    // Reset configuration to default
+    // TODO: maybe move this on serial communication setup
+    if (!resetConfiguration())
+    {
+        return false;
+    }
+
+    // Disable NMEA messages
+    if (!disableNMEAMessages())
+    {
+        return false;
+    }
+
+    Thread::sleep(100);
+
+    // Set GNSS configuration
+    if (!setGNSSConfiguration())
+    {
+        return false;
+    }
+
+    Thread::sleep(100);
+
+    // Enable UBX messages
+    if (!enableUBXMessages())
+    {
+        return false;
+    }
+
+    Thread::sleep(100);
+
+    // Set rate
+    if (!setRate())
+    {
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::selfTest() { return true; }
+
+UbloxGPSData UbloxGPS::sampleImpl()
+{
+    Lock<FastMutex> l(mutex);
+    return threadSample;
+}
+
+void UbloxGPS::run()
+{
+    /**
+     * UBX message structure:
+     * - 2B: Preamble
+     * - 1B: Message class
+     * - 1B: Message id
+     * - 2B: Payload length
+     * - lB: Payload
+     * - 2B: Checksum
+     */
+    uint8_t message[6 + UBX_MAX_PAYLOAD_LENGTH + 2];
+    uint16_t payloadLength;
+
+    while (!shouldStop())
+    {
+        StackLogger::getInstance().updateStack(THID_GPS);
+
+        // Try to read the message
+        if (!readUBXMessage(message, payloadLength))
+        {
+            LOG_DEBUG(logger, "Unable to read a UBX message");
+            continue;
+        }
+
+        // Parse the message
+        if (!parseUBXMessage(message))
+        {
+            LOG_DEBUG(logger,
+                      "UBX message not recognized (class:0x{02x}, id: 0x{02x})",
+                      message[2], message[3]);
+        }
+    }
+}
+
+void UbloxGPS::ubxChecksum(uint8_t* msg, int len)
+{
+    uint8_t ck_a = 0, ck_b = 0;
+
+    // The length must be valid, at least 8 bytes (preamble, msg, length,
+    // checksum)
+    if (len <= 8)
+    {
+        return;
+    }
+
+    // Checksum calculation from byte 2 to end of payload
+    for (int i = 2; i < len - 2; i++)
+    {
+        ck_a = ck_a + msg[i];
+        ck_b = ck_b + ck_a;
+    }
+
+    msg[len - 2] = ck_a;
+    msg[len - 1] = ck_b;
+}
+
+bool UbloxGPS::writeUBXMessage(uint8_t* message, int length)
+{
+    // Compute the checksum
+    ubxChecksum(message, length);
+
+    // Write configuration
+    if (write(gpsFile, message, length) < 0)
+    {
+        LOG_ERR(logger,
+                "Failed to write ubx message (class:0x{02x}, id: 0x{02x})",
+                message[2], message[3]);
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::serialCommuinicationSetup()
+{
+    intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
+
+    // Change the baudrate only if it is different than the default
+    if (baudrate != defaultBaudrate)
+    {
+        // Close the gps file if already opened
+        devFs->remove(serialPortName);
+
+        // Open the serial port device with the default boudrate
+        if (!devFs->addDevice(serialPortName,
+                              intrusive_ref_ptr<Device>(new STM32Serial(
+                                  serialPortNumber, defaultBaudrate))))
+        {
+            LOG_ERR(logger,
+                    "[gps] Faild to open serial port {0} with baudrate {1} as "
+                    "file {2}",
+                    serialPortNumber, defaultBaudrate, serialPortName);
+            return false;
+        }
+
+        // Open the gps file
+        if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+        {
+            LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+            return false;
+        }
+
+        // Change boudrate
+        if (!setBaudrate())
+        {
+            return false;
+        };
+
+        // Close the gps file
+        if (close(gpsFile) < 0)
+        {
+            LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath);
+            return false;
+        }
+
+        // Close the serial port
+        if (!devFs->remove(serialPortName))
+        {
+            LOG_ERR(logger, "Failed to close serial port {} as file {}",
+                    serialPortNumber, serialPortName);
+            return false;
+        }
+    }
+
+    // Reopen the serial port with the configured boudrate
+    if (!devFs->addDevice(serialPortName,
+                          intrusive_ref_ptr<Device>(
+                              new STM32Serial(serialPortNumber, baudrate))))
+    {
+        LOG_ERR(logger,
+                "Faild to open serial port {} with baudrate {} as file {}\n",
+                serialPortNumber, defaultBaudrate, serialPortName);
+        return false;
+    }
+
+    // Reopen the gps file
+    if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+    {
+        LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::resetConfiguration()
+{
+    uint8_t ubx_cfg_prt[RESET_CONFIG_MSG_LEN] = {
+        0Xb5, 0x62,  // Preamble
+        0x06, 0x04,  // Message UBX-CFG-RST
+        0x04, 0x00,  // Length
+        0x00, 0x00,  // navBbrMask (Hot start)
+        0x00,        // Hardware reset immediately
+        0x00,        // Reserved
+        0xff, 0xff   // Checksum
+    };
+
+    return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN);
+}
+
+bool UbloxGPS::setBaudrate()
+{
+    uint8_t ubx_cfg_prt[SET_BAUDRATE_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x0c, 0x00,              // Length
+        0x00,                    // Version
+        0xff,                    // All layers
+        0x00, 0x00,              // Reserved
+        0x01, 0x00, 0x52, 0x40,  // Configuration item key ID
+        0xff, 0xff, 0xff, 0xff,  // Value
+        0xff, 0xff               // Checksum
+    };
+
+    // Prepare boud rate
+    ubx_cfg_prt[14] = baudrate;
+    ubx_cfg_prt[15] = baudrate >> 8;
+    ubx_cfg_prt[16] = baudrate >> 16;
+    ubx_cfg_prt[17] = baudrate >> 24;
+
+    return writeUBXMessage(ubx_cfg_prt, SET_BAUDRATE_MSG_LEN);
+}
+
+bool UbloxGPS::disableNMEAMessages()
+{
+    uint8_t ubx_cfg_valset[DISABLE_NMEA_MESSAGES_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x22, 0x00,              // Length
+        0x00,                    // Version
+        0xff,                    // All layers
+        0x00, 0x00,              // Reserved
+        0xbb, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_GGA_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_GGA_UART1 value
+        0xca, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_GLL_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_GLL_UART1 value
+        0xc0, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_GSA_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_GSA_UART1 value
+        0xc5, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_GSV_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_GSV_UART1 value
+        0xac, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_RMC_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_RMC_UART1 value
+        0xb1, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_VTG_UART1 key ID
+        0x00,                    // CFG-MSGOUT-NMEA_ID_VTG_UART1 value
+        0xff, 0xff               // Checksum
+    };
+
+    return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN);
+}
+
+bool UbloxGPS::setGNSSConfiguration()
+{
+    uint8_t ubx_cfg_valset[SET_GNSS_CONF_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x09, 0x00,              // Length
+        0x00,                    // Version
+        0x07,                    // All layers
+        0x00, 0x00,              // Reserved
+        0x21, 0x00, 0x11, 0x20,  // CFG-NAVSPG-DYNMODEL key ID
+        0x08,                    // CFG-NAVSPG-DYNMODEL value
+        0xff, 0xff               // Checksum
+    };
+
+    return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN);
+}
+
+bool UbloxGPS::enableUBXMessages()
+{
+    uint8_t ubx_cfg_valset[ENABLE_UBX_MESSAGES_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x09, 0x00,              // Length
+        0x00,                    // Version
+        0xff,                    // All layers
+        0x00, 0x00,              // Reserved
+        0x07, 0x00, 0x91, 0x20,  // CFG-MSGOUT-UBX_NAV_PVT_UART1 key ID
+        0x01,                    // CFG-MSGOUT-UBX_NAV_PVT_UART1 value
+        0xff, 0xff               // Checksum
+    };
+
+    return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN);
+}
+
+bool UbloxGPS::setRate()
+{
+    uint16_t rate = 1000 / sampleRate;
+    LOG_DEBUG(logger, "Rate: {}", rate);
+
+    uint8_t ubx_cfg_valset[SET_RATE_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x0a, 0x00,              // Length
+        0x00,                    // Version
+        0x07,                    // All layers
+        0x00, 0x00,              // Reserved
+        0x01, 0x00, 0x21, 0x30,  // CFG-RATE-MEAS key ID
+        0xff, 0xff,              // CFG-RATE-MEAS value
+        0xff, 0xff               // Checksum
+    };
+
+    // Prepare rate
+    ubx_cfg_valset[14] = rate;
+    ubx_cfg_valset[15] = rate >> 8;
+
+    return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN);
+}
+
+bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength)
+{
+    // Read preamble
+    do
+    {
+        // Read util the first byte of the preamble
+        do
+        {
+            if (read(gpsFile, &message[0], 1) <= 0)  // No more data available
+            {
+                return false;
+            }
+        } while (message[0] != PREAMBLE[0]);
+
+        // Read the next byte
+        if (read(gpsFile, &message[1], 1) <= 0)  // No more data available
+        {
+            return false;
+        }
+    } while (message[1] != PREAMBLE[1]);  // Continue
+
+    // Read message class and ID
+    if (read(gpsFile, &message[2], 1) <= 0)
+    {
+        return false;
+    }
+    if (read(gpsFile, &message[3], 1) <= 0)
+    {
+        return false;
+    }
+
+    // Read length
+    if (read(gpsFile, &message[4], 2) <= 0)
+    {
+        return false;
+    }
+    payloadLength = message[4] | (message[5] << 8);
+    if (payloadLength > UBX_MAX_PAYLOAD_LENGTH)
+    {
+        return false;
+    }
+
+    // Read paylaod and checksum
+    for (auto i = 0; i < payloadLength + 2; i++)
+    {
+        if (read(gpsFile, &message[6 + i], 1) <= 0)
+        {
+            return false;
+        }
+    }
+
+    // Verify the checksum
+    uint8_t msgChecksum1 = message[6 + payloadLength];
+    uint8_t msgChecksum2 = message[6 + payloadLength + 1];
+    ubxChecksum(message, 6 + payloadLength + 2);
+    if (msgChecksum1 != message[6 + payloadLength] ||
+        msgChecksum2 != message[6 + payloadLength + 1])
+    {
+        LOG_ERR(logger, "Message checksum verification failed");
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::parseUBXMessage(uint8_t* message)
+{
+    switch (message[2])  // Message class
+    {
+        case 0x01:  // UBX-NAV
+            return parseUBXNAVMessage(message);
+        case 0x05:  // UBX-ACK
+            return parseUBXACKMessage(message);
+    }
+    return false;
+}
+
+bool UbloxGPS::parseUBXNAVMessage(uint8_t* message)
+{
+    switch (message[3])  // Message id
+    {
+        case 0x07:  // UBX-NAV-PVT
+            // Lock the threadSample variable
+            Lock<FastMutex> l(mutex);
+
+            // Latitude
+            int32_t rawLatitude = message[6 + 28] | message[6 + 29] << 8 |
+                                  message[6 + 30] << 16 | message[6 + 31] << 24;
+            threadSample.latitude = (float)rawLatitude / 1e7;
+
+            // Longitude
+            int32_t rawLongitude = message[6 + 24] | message[6 + 25] << 8 |
+                                   message[6 + 26] << 16 |
+                                   message[6 + 27] << 24;
+            threadSample.longitude = (float)rawLongitude / 1e7;
+
+            // Height
+            int32_t rawHeight = message[6 + 32] | message[6 + 33] << 8 |
+                                message[6 + 34] << 16 | message[6 + 35] << 24;
+            threadSample.height = (float)rawHeight / 1e3;
+
+            // Velocity north
+            int32_t rawVelocityNorth = message[6 + 48] | message[6 + 49] << 8 |
+                                       message[6 + 50] << 16 |
+                                       message[6 + 51] << 24;
+            threadSample.velocityNorth = (float)rawVelocityNorth / 1e3;
+
+            // Velocity east
+            int32_t rawVelocityEast = message[6 + 52] | message[6 + 53] << 8 |
+                                      message[6 + 54] << 16 |
+                                      message[6 + 55] << 24;
+            threadSample.velocityEast = (float)rawVelocityEast / 1e3;
+
+            // Velocity down
+            int32_t rawVelocityDown = message[6 + 56] | message[6 + 57] << 8 |
+                                      message[6 + 58] << 16 |
+                                      message[6 + 59] << 24;
+            threadSample.velocityDown = (float)rawVelocityDown / 1e3;
+
+            // Speed
+            int32_t rawSpeed = message[6 + 60] | message[6 + 61] << 8 |
+                               message[6 + 62] << 16 | message[6 + 63] << 24;
+            threadSample.speed = (float)rawSpeed / 1e3;
+
+            // Track (heading of motion)
+            int32_t rawTrack = message[6 + 64] | message[6 + 65] << 8 |
+                               message[6 + 66] << 16 | message[6 + 67] << 24;
+            threadSample.track = (float)rawTrack / 1e5;
+
+            // Number of satellite
+            threadSample.satellites = (uint8_t)message[6 + 23];
+
+            // Fix (every type of fix accepted)
+            threadSample.fix = message[6 + 20] != 0;
+
+            // Timestamp
+            threadSample.gpsTimestamp =
+                TimestampTimer::getInstance().getTimestamp();
+
+            return true;
+    }
+
+    return false;
+}
+
+bool UbloxGPS::parseUBXACKMessage(uint8_t* message)
+{
+    switch (message[3])  // Message id
+    {
+        case 0x00:  // UBX-ACK-NAC
+            LOG_DEBUG(logger,
+                      "Received NAC for message (class:0x{02x}, id: 0x{02x})",
+                      message[6], message[7]);
+            return true;
+        case 0x01:  // UBX-ACK-ACK
+            LOG_DEBUG(logger,
+                      "Received ACK for message (class:0x{02x}, id: 0x{02x})",
+                      message[6], message[7]);
+            return true;
+    }
+    return false;
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h
new file mode 100644
index 0000000000000000000000000000000000000000..9495bc6f5fe792774f0bb9a59c32f35c6e97e5b8
--- /dev/null
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h
@@ -0,0 +1,151 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, 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 <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <miosix.h>
+#include <sensors/Sensor.h>
+
+#include "UbloxGPSData.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Driver for Ublox GPSs
+ *
+ * This driver handles communication and setup with Ublox GPSs. It uses the
+ * binary UBX protocol to retrieve and parse navigation data quicker than using
+ * the string based NMEA.
+ *
+ * At initialization it configures the device with the specified baudrate, reset
+ * the configuration and sets up UBX messages and GNSS parameters.
+ *
+ * Communication with the device is performed through a file, the driver opens
+ * the serial port under the filepath /dev/<serialPortName>.
+ * There is no need for the file to be setted up beforhand.
+ *
+ * This driver was written for a NEO-M9N gps with the latest version of UBX.
+ */
+class UbloxGPS : public Sensor<UbloxGPSData>, public ActiveObject
+{
+public:
+    /**
+     * @brief Construct a new UbloxGPS object
+     *
+     * @param boudrate_ Baudrate to communicate with the device (max: 921600,
+     * min: 4800 for NEO-M9N)
+     * @param sampleRate_ GPS sample rate (max: 25 for NEO-M9N)
+     * @param serialPortName_ Name of the file for the gps device
+     * @param defaultBaudrate_ Startup baudrate (38400 for NEO-M9N)
+     */
+    UbloxGPS(int boudrate_ = 921600, uint8_t sampleRate_ = 10,
+             int serialPortNumber_ = 2, const char *serialPortName_ = "gps",
+             int defaultBaudrate_ = 38400);
+
+    /**
+     * @brief Sets up the serial port baudrate, disables the NMEA messages,
+     * configures GNSS options and enables UBX-PVT message
+     *
+     * @return True if the operation succeeded
+     */
+    bool init() override;
+
+    /**
+     * @brief Read a single message form the GPS, waits 2 sample cycle
+     *
+     * @return True if a message was sampled
+     */
+    bool selfTest() override;
+
+private:
+    UbloxGPSData sampleImpl() override;
+
+    void run() override;
+
+    void ubxChecksum(uint8_t *msg, int len);
+
+    /**
+     * @brief Compute the checksum and write the message to the device
+     */
+    bool writeUBXMessage(uint8_t *message, int length);
+
+    /**
+     * @brief Sets up the serial port with the correct baudrate
+     *
+     * Opens the serial port with the defaul baudrate and changes it to
+     * the value specified in the constructor, then it reopens the serial port.
+     * If the device is already using the correct baudrate this won't have
+     * effect. However if the gps is using a different baudrate the diver won't
+     * be able to communicate.
+     */
+    bool serialCommuinicationSetup();
+
+    bool resetConfiguration();
+
+    bool setBaudrate();
+
+    bool disableNMEAMessages();
+
+    bool setGNSSConfiguration();
+
+    bool enableUBXMessages();
+
+    bool setRate();
+
+    bool readUBXMessage(uint8_t *message, uint16_t &payloadLength);
+
+    bool parseUBXMessage(uint8_t *message);
+
+    bool parseUBXNAVMessage(uint8_t *message);
+
+    bool parseUBXACKMessage(uint8_t *message);
+
+    const int baudrate;        // [baud]
+    const uint8_t sampleRate;  // [Hz]
+    const int serialPortNumber;
+    const char *serialPortName;
+    const int defaultBaudrate;  // [baud]
+
+    char gpsFilePath[16];  ///< Allows for a filename of up to 10 characters
+    int gpsFile = -1;
+
+    mutable miosix::FastMutex mutex;
+    UbloxGPSData threadSample{};
+
+    static constexpr int SET_BAUDRATE_MSG_LEN          = 20;
+    static constexpr int RESET_CONFIG_MSG_LEN          = 12;
+    static constexpr int DISABLE_NMEA_MESSAGES_MSG_LEN = 42;
+    static constexpr int SET_GNSS_CONF_LEN             = 17;
+    static constexpr int ENABLE_UBX_MESSAGES_MSG_LEN   = 17;
+    static constexpr int SET_RATE_MSG_LEN              = 18;
+
+    static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62};
+
+    static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92;
+
+    PrintLogger logger = Logging::getLogger("ubloxgps");
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f5570000b78b915a956b96e754d784e08f7611f
--- /dev/null
+++ b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
@@ -0,0 +1,47 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Author: Davide Bonomini
+ *
+ * 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 UbloxGPSData : public GPSData
+{
+    static std::string header()
+    {
+        return "gps_timestamp,latitude,longitude,height,velocity_north,"
+               "velocity_east,velocity_down,speed,track,num_satellites,fix\n";
+    }
+
+    void print(std::ostream &os) const
+    {
+        os << gpsTimestamp << "," << latitude << "," << longitude << ","
+           << height << "," << velocityNorth << "," << velocityEast << ","
+           << velocityDown << "," << speed << "," << track << ","
+           << (int)satellites << "," << (int)fix << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp
index cb97c2b0818efdf597c3e0b270b774e67c212c96..eb9d3e9043d62522ec566c40e501d139403ee1d4 100644
--- a/src/shared/sensors/VN100/VN100.cpp
+++ b/src/shared/sensors/VN100/VN100.cpp
@@ -27,7 +27,8 @@
 namespace Boardcore
 {
 
-VN100::VN100(unsigned int portNumber, BaudRates baudRate, CRCOptions crc)
+VN100::VN100(USARTType *portNumber, USARTInterface::Baudrate baudRate,
+             CRCOptions crc)
     : portNumber(portNumber), baudRate(baudRate), crc(crc)
 {
 }
@@ -128,8 +129,7 @@ bool VN100::sampleRaw()
     }
 
     // Send the IMU sampling command
-    if (!(serialInterface->send(preSampleImuString->c_str(),
-                                preSampleImuString->length())))
+    if (!(serialInterface->writeString(preSampleImuString->c_str())))
     {
         LOG_WARN(logger, "Unable to sample due to serial communication error");
         return false;
@@ -177,13 +177,6 @@ bool VN100::closeAndReset()
         return false;
     }
 
-    // Close the serial
-    if (!(serialInterface->closeSerial()))
-    {
-        LOG_WARN(logger, "Impossible to close vn100 serial communication");
-        return false;
-    }
-
     isInit = false;
 
     // Free the recvString memory
@@ -224,8 +217,7 @@ VN100Data VN100::sampleImpl()
     }
 
     // Returns Quaternion, Magnetometer, Accelerometer and Gyro
-    if (!(serialInterface->send(preSampleImuString->c_str(),
-                                preSampleImuString->length())))
+    if (!(serialInterface->writeString(preSampleImuString->c_str())))
     {
         // If something goes wrong i return the last sampled data
         return lastSample;
@@ -257,8 +249,7 @@ VN100Data VN100::sampleImpl()
     // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
     // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
     // GYROSCOPE VALUES
-    if (!(serialInterface->send(preSampleTempPressString->c_str(),
-                                preSampleTempPressString->length())))
+    if (!(serialInterface->writeString(preSampleTempPressString->c_str())))
     {
         // If something goes wrong i return the last sampled data
         return lastSample;
@@ -312,16 +303,10 @@ bool VN100::disableAsyncMessages(bool waitResponse)
 bool VN100::configDefaultSerialPort()
 {
     // Initial default settings
-    serialInterface = new VN100Serial(
-        portNumber, static_cast<unsigned int>(BaudRates::Baud_115200));
+    serialInterface = new USART(portNumber, USARTInterface::Baudrate::B115200);
 
     // Check correct serial init
-    if (!serialInterface->init())
-    {
-        return false;
-    }
-
-    return true;
+    return serialInterface->init();
 }
 
 /**
@@ -333,7 +318,7 @@ bool VN100::configUserSerialPort()
     std::string command;
 
     // I format the command to change baud rate
-    command = fmt::format("{}{}", "VNWRG,5,", baudRate);
+    command = fmt::format("{}{}", "VNWRG,5,", static_cast<int>(baudRate));
 
     // I can send the command
     if (!sendStringCommand(command))
@@ -341,23 +326,14 @@ bool VN100::configUserSerialPort()
         return false;
     }
 
-    // I can close the serial
-    serialInterface->closeSerial();
-
     // Destroy the serial object
     delete serialInterface;
 
     // I can open the serial with user's baud rate
-    serialInterface =
-        new VN100Serial(portNumber, static_cast<unsigned int>(baudRate));
+    serialInterface = new USART(portNumber, baudRate);
 
     // Check correct serial init
-    if (!serialInterface->init())
-    {
-        return false;
-    }
-
-    return true;
+    return serialInterface->init();
 }
 
 bool VN100::setCrc(bool waitResponse)
@@ -443,11 +419,13 @@ bool VN100::selfTestImpl()
 
     if (!sendStringCommand("VNRRG,01"))
     {
+        LOG_WARN(logger, "Unable to send string command");
         return false;
     }
 
     if (!recvStringCommand(recvString, recvStringMaxDimension))
     {
+        LOG_WARN(logger, "Unable to receive string command");
         return false;
     }
 
@@ -661,14 +639,14 @@ bool VN100::sendStringCommand(std::string command)
     }
 
     // I send the final command
-    if (!(serialInterface->send(command.c_str(), command.length() + 1)))
+    if (!serialInterface->writeString(command.c_str()))
     {
         return false;
     }
 
     // Wait some time
     // TODO dimension the time
-    miosix::Thread::sleep(1);
+    miosix::Thread::sleep(500);
 
     return true;
 }
@@ -677,7 +655,7 @@ bool VN100::recvStringCommand(char *command, int maxLength)
 {
     int i = 0;
     // Read the buffer
-    if (!(serialInterface->recv(command, maxLength)))
+    if (!(serialInterface->read(command, maxLength)))
     {
         return false;
     }
diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100.h
index 9d40dc304d1de2101beecf71664ecbb7a806fb30..aa61dbdd9b458a96b411047e66452912337e6adc 100644
--- a/src/shared/sensors/VN100/VN100.h
+++ b/src/shared/sensors/VN100/VN100.h
@@ -26,7 +26,7 @@
  * @brief Driver for the VN100S IMU.
  *
  * The VN100S sensor is a calibrated IMU which includes accelerometer,
- * magnetometer, gyroscope, barometer and temperature sensor. ùThe device
+ * magnetometer, gyroscope, barometer and temperature sensor. The device
  * provides also a calibration matrix and an anti-drift matrix for the gyroscope
  * values. The goal of this driver though is to interface the sensor in its
  * basic use. Things like asynchronous data and anti-drift techniques haven't
@@ -58,7 +58,7 @@
 #include <utils/Debug.h>
 
 #include "VN100Data.h"
-#include "VN100Serial.h"
+#include "drivers/usart/USART.h"
 
 namespace Boardcore
 {
@@ -69,19 +69,6 @@ namespace Boardcore
 class VN100 : public Sensor<VN100Data>
 {
 public:
-    enum class BaudRates : unsigned int
-    {
-        Baud_9600   = 9600,
-        Baud_19200  = 19200,
-        Baud_38400  = 38400,
-        Baud_57600  = 57600,
-        Baud_115200 = 115200,
-        Baud_128000 = 128000,
-        Baud_230400 = 230400,
-        Baud_460800 = 460800,
-        Baud_921600 = 921600
-    };
-
     enum class CRCOptions : uint8_t
     {
         CRC_NO        = 0x00,
@@ -93,12 +80,13 @@ public:
      * @brief Constructor.
      *
      * @param USART port number.
-     * @param BaudRate different from the sensor's default.
+     * @param BaudRate different from the sensor's default [9600, 19200, 38400,
+     * 57600, 115200, 128000, 230400, 460800, 921600].
      * @param Redundancy check option.
      */
-    VN100(unsigned int portNumber = defaultPortNumber,
-          BaudRates baudRate      = BaudRates::Baud_115200,
-          CRCOptions crc          = CRCOptions::CRC_ENABLE_8);
+    VN100(USARTType *portNumber    = USART2,
+          USART::Baudrate baudRate = USART::Baudrate::B921600,
+          CRCOptions crc           = CRCOptions::CRC_ENABLE_8);
 
     bool init() override;
 
@@ -236,8 +224,8 @@ private:
      */
     uint16_t calculateChecksum16(uint8_t *message, int length);
 
-    unsigned int portNumber;
-    BaudRates baudRate;
+    USARTType *portNumber;
+    USART::Baudrate baudRate;
     CRCOptions crc;
     bool isInit = false;
 
@@ -267,11 +255,10 @@ private:
      * @brief Serial interface that is needed to communicate
      * with the sensor via ASCII codes.
      */
-    VN100Serial *serialInterface = nullptr;
+    USARTInterface *serialInterface = nullptr;
 
     PrintLogger logger = Logging::getLogger("vn100");
 
-    static const unsigned int defaultPortNumber      = 2;
     static const unsigned int recvStringMaxDimension = 200;
 };
 }  // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100Data.h
index 5671733e8a479513c71b05df7dcc317f15c20bc0..210a3b82d77b5788afeb00a808a345c7dc1f6fad 100644
--- a/src/shared/sensors/VN100/VN100Data.h
+++ b/src/shared/sensors/VN100/VN100Data.h
@@ -53,6 +53,7 @@ struct VN100Data : public QuaternionData,
     /**
      * @brief Void parameters constructor
      */
+    // cppcheck-suppress uninitDerivedMemberVar
     VN100Data()
         : QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0,
                                                                   0.0},
@@ -67,35 +68,24 @@ struct VN100Data : public QuaternionData,
      * @param single data structures for all the data
      */
     // cppcheck-suppress passedByValue
+    // cppcheck-suppress uninitDerivedMemberVar
     VN100Data(QuaternionData quat, MagnetometerData magData,
               AccelerometerData accData, GyroscopeData gyro,
               TemperatureData temp, PressureData pres)
-        : QuaternionData{quat.quatTimestamp, quat.quatX, quat.quatY, quat.quatZ,
-                         quat.quatW},
-          MagnetometerData{magData.magneticFieldTimestamp,
-                           magData.magneticFieldX, magData.magneticFieldY,
-                           magData.magneticFieldZ},
-          AccelerometerData{accData.accelerationTimestamp,
-                            accData.accelerationX, accData.accelerationY,
-                            accData.accelerationZ},
-          GyroscopeData{gyro.angularVelocityTimestamp, gyro.angularVelocityX,
-                        gyro.angularVelocityY, gyro.angularVelocityZ},
-          TemperatureData{temp.temperatureTimestamp, temp.temperature},
-          PressureData{pres.pressureTimestamp, pres.pressure}
+        : QuaternionData(quat), MagnetometerData(magData),
+          AccelerometerData(accData), GyroscopeData(gyro),
+          TemperatureData(temp), PressureData(pres)
     {
     }
 
     static std::string header()
     {
-        return "quatTimestamp,quatX,quatY,quatZ,quatW,"
-               "magneticFieldTimestamp,magneticFieldX,"
-               "magneticFieldY,magneticFieldZ,"
+        return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp,"
+               "magneticFieldX,magneticFieldY,magneticFieldZ,"
                "accelerationTimestamp,accelerationX,accelerationY,"
-               "accelerationZ,gyro_"
-               "timestamp,"
-               "angularVelocityX,"
-               "angularVelocityY,angularVelocityZ"
-               "temperatureTimestamp,temp,pressureTimestamp,press\n";
+               "accelerationZ,gyro_timestamp,angularVelocityX,angularVelocityY,"
+               "angularVelocityZ,temperatureTimestamp,temperature,"
+               "pressureTimestamp,pressure\n";
     }
 
     void print(std::ostream& os) const
diff --git a/src/shared/sensors/analog/loadcell/LoadCell.h b/src/shared/sensors/analog/AnalogLoadCell.h
similarity index 85%
rename from src/shared/sensors/analog/loadcell/LoadCell.h
rename to src/shared/sensors/analog/AnalogLoadCell.h
index 76d678ce2304542247fb0fa57b33aed73972ff4d..f554bee330b2595a382b9c5cddc7bb7a44699282 100644
--- a/src/shared/sensors/analog/loadcell/LoadCell.h
+++ b/src/shared/sensors/analog/AnalogLoadCell.h
@@ -26,15 +26,15 @@
 
 #include <functional>
 
-#include "LoadCellData.h"
+#include "AnalogLoadCellData.h"
 
 namespace Boardcore
 {
 
-class LoadCellSensor : public Sensor<LoadCellData>
+class AnalogLoadCell : public Sensor<AnalogLoadCellData>
 {
 public:
-    LoadCellSensor(std::function<std::pair<uint64_t, float>()> getVoltage,
+    AnalogLoadCell(std::function<std::pair<uint64_t, float>()> getVoltage,
                    const float mVtoV, const unsigned int fullScale,
                    const float supplyVoltage = 5)
         : getVoltage(getVoltage),
@@ -46,11 +46,11 @@ public:
 
     bool selfTest() override { return true; };
 
-    LoadCellData sampleImpl() override
+    AnalogLoadCellData sampleImpl() override
     {
-        LoadCellData data;
+        AnalogLoadCellData data;
 
-        std::tie(data.timestamp, data.voltage) = getVoltage();
+        std::tie(data.loadTimestamp, data.voltage) = getVoltage();
 
         data.load = data.voltage / conversionCoeff;
 
diff --git a/src/shared/sensors/analog/loadcell/LoadCellData.h b/src/shared/sensors/analog/AnalogLoadCellData.h
similarity index 85%
rename from src/shared/sensors/analog/loadcell/LoadCellData.h
rename to src/shared/sensors/analog/AnalogLoadCellData.h
index 665ae93111656de0f1c756d2a4003d27093fd1fc..8cb8a7274ad3e2cd0101b43a2b7c70742addaa04 100644
--- a/src/shared/sensors/analog/loadcell/LoadCellData.h
+++ b/src/shared/sensors/analog/AnalogLoadCellData.h
@@ -22,22 +22,20 @@
 
 #pragma once
 
-#include <ostream>
+#include <sensors/SensorData.h>
 
 namespace Boardcore
 {
 
-struct LoadCellData
+struct AnalogLoadCellData : LoadCellData
 {
-    uint64_t timestamp;
     float voltage;
-    float load;
 
-    static std::string header() { return "timestamp,voltage,load\n"; }
+    static std::string header() { return "loadTimestamp,load,voltage\n"; }
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << voltage << "," << load << "\n";
+        os << loadTimestamp << "," << load << "," << voltage << "\n";
     }
 };
 
diff --git a/src/shared/sensors/analog/battery/BatteryVoltageSensor.h b/src/shared/sensors/analog/BatteryVoltageSensor.h
similarity index 100%
rename from src/shared/sensors/analog/battery/BatteryVoltageSensor.h
rename to src/shared/sensors/analog/BatteryVoltageSensor.h
diff --git a/src/shared/sensors/analog/battery/BatteryVoltageSensorData.h b/src/shared/sensors/analog/BatteryVoltageSensorData.h
similarity index 100%
rename from src/shared/sensors/analog/battery/BatteryVoltageSensorData.h
rename to src/shared/sensors/analog/BatteryVoltageSensorData.h
diff --git a/src/shared/sensors/analog/current/CurrentSensor.h b/src/shared/sensors/analog/CurrentSensor.h
similarity index 100%
rename from src/shared/sensors/analog/current/CurrentSensor.h
rename to src/shared/sensors/analog/CurrentSensor.h
diff --git a/src/shared/sensors/analog/current/CurrentSensorData.h b/src/shared/sensors/analog/CurrentSensorData.h
similarity index 100%
rename from src/shared/sensors/analog/current/CurrentSensorData.h
rename to src/shared/sensors/analog/CurrentSensorData.h
diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
index 24e9dd136665c371bf48b6a8ac3b2054ee4424de..1e2785023c487613b58f094d19113e9d5fe51c6f 100644
--- a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
+++ b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h
@@ -22,7 +22,7 @@
 
 #pragma once
 
-#include <math/Stats.h>
+#include <utils/Stats/Stats.h>
 
 #include "HoneywellPressureSensor.h"
 #include "SSCDRRN015PDAData.h"
diff --git a/src/shared/sensors/calibration/AxisOrientation.h b/src/shared/sensors/calibration/AxisOrientation.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b6924e7f9ebb9878995dcec88e2cc65345b8fd4
--- /dev/null
+++ b/src/shared/sensors/calibration/AxisOrientation.h
@@ -0,0 +1,233 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, 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/Sensor.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace Boardcore
+{
+
+/**
+ * This enum act like versors towards the chosen axis.
+ *
+ * X, Y and Z always set according to the right hand rule, so that:
+ * X is the index
+ * Y is the second finger
+ * Z is the thumb
+ *
+ */
+enum class Direction : uint8_t
+{
+    POSITIVE_X = 0,
+    NEGATIVE_X,
+    POSITIVE_Y,
+    NEGATIVE_Y,
+    POSITIVE_Z,
+    NEGATIVE_Z,
+};
+
+constexpr const char* humanFriendlyDirection[]{
+    "North", "South", "East", "West", "Down", "Up",
+};
+
+inline Eigen::Vector3f orientationToVector(Direction val)
+{
+    switch (val)
+    {
+        case Direction::POSITIVE_X:
+            return {1, 0, 0};
+        case Direction::NEGATIVE_X:
+            return {-1, 0, 0};
+        case Direction::POSITIVE_Y:
+            return {0, 1, 0};
+        case Direction::NEGATIVE_Y:
+            return {0, -1, 0};
+        case Direction::POSITIVE_Z:
+            return {0, 0, 1};
+        case Direction::NEGATIVE_Z:
+            return {0, 0, -1};
+        default:
+            /* never happens, added just to shut up the warnings */
+            return {0, 0, 0};
+    }
+}
+
+/**
+ * This struct represents in the most general way any kind of transformation of
+ * the reference frame (axis X, Y and Z).
+ * This data type is intended to simplify the code, so you shouldn't instantiate
+ * this struct directly, but rather use the structures AxisAngleOrientation or
+ * AxisOrthoOrientation that will be automatically corrected to this one, thanks
+ * to the implicit cast in favor of AxisOrientation.
+ *
+ * For example:
+ *
+ * AxisAngleOrientation angles ( PI/2, PI, 0);
+ * AxisOrthoOrientation ortho  ( Direction::NEGATIVE_X,
+ * Direction::POSITIVE_Z );
+ *
+ * // The implicit cast is supported and recommended
+ * AxisOrientation converted1 = angles;
+ * AxisOrientation converted2 = ortho;
+ *
+ * // Now we can use the generated matrix:
+ * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 }
+ *
+ */
+struct AxisOrientation
+{
+    Eigen::Matrix3f mat;
+
+    AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {}
+
+    AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {}
+
+    void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; }
+
+    Eigen::Matrix3f getMatrix() const { return mat; }
+};
+
+/**
+ * This struct uses the three angles yaw, pitch and roll to define the
+ * transformation of the reference frame, so according to N.E.D standard we get:
+ *
+ *         X (north)
+ *        /
+ *       /
+ *      .----> Y (east)
+ *      |
+ *      |
+ *      v
+ *     Z (down)
+ *
+ * Where:
+ * Yaw is rotation of Z axis
+ * Pitch is rotation of Y axis
+ * Roll is rotation of X axis
+ */
+struct AxisAngleOrientation
+{
+    float yaw, pitch, roll;
+
+    AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {}
+
+    AxisAngleOrientation(float _yaw, float _pitch, float _roll)
+        : yaw(_yaw), pitch(_pitch), roll(_roll)
+    {
+    }
+
+    operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
+
+    Eigen::Matrix3f getMatrix() const
+    {
+        return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) *
+                Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) *
+                Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0}))
+            .toRotationMatrix();
+    }
+};
+
+/**
+ * This struct represents the orientation of the reference frame relative
+ * to X, Y, Z in the start orientation.
+ * If we know the orientation of the X and Y axis, using the right hand rule
+ * we can infer the Z axis.
+ *
+ * For example, if the base reference is:
+ *
+ *        z
+ *        ^
+ *        |
+ *        |
+ *        /----> y
+ *       /
+ *      x
+ *
+ * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get:
+ *
+ *          y   z
+ *          ^   ^
+ *          |  /
+ *          | /
+ *   x <----/
+ *
+ */
+struct AxisOrthoOrientation
+{
+    Direction xAxis, yAxis;
+
+    AxisOrthoOrientation()
+        : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y)
+    {
+    }
+
+    AxisOrthoOrientation(Direction _xAxis, Direction _yAxis)
+        : xAxis(_xAxis), yAxis(_yAxis)
+    {
+    }
+
+    operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
+
+    Eigen::Matrix3f getMatrix() const
+    {
+        Eigen::Vector3f vx, vy, vz;
+
+        vx = orientationToVector(xAxis);
+        vy = orientationToVector(yAxis);
+        vz = vx.cross(vy);
+
+        Eigen::Matrix3f mat;
+        mat.col(0) << vx;
+        mat.col(1) << vy;
+        mat.col(2) << vz;
+        return mat;
+    }
+};
+
+/**
+ * This struct represents axis orientation relative to a reference system.
+ * Operatively it simply combines two transformations. It is particularly useful
+ * to obtain an AxisOrientation from a reference system generally not N.E.D.
+ */
+struct AxisRelativeOrientation
+{
+    AxisOrientation systemOrientation, orientation;
+
+    AxisRelativeOrientation(const AxisOrientation& _systemOrientation,
+                            const AxisOrientation& _orientation)
+        : systemOrientation(_systemOrientation), orientation(_orientation)
+    {
+    }
+
+    operator AxisOrientation() const { return {getMatrix()}; }
+
+    Eigen::Matrix3f getMatrix() const
+    {
+        return systemOrientation.getMatrix() * orientation.getMatrix();
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/BiasCalibration.h b/src/shared/sensors/calibration/BiasCalibration.h
index 5fc2ed071900c11446e6e5d25c27f34ba050e2b2..be84f860e0118b37dff9bc6e4b6c600ff74aa473 100644
--- a/src/shared/sensors/calibration/BiasCalibration.h
+++ b/src/shared/sensors/calibration/BiasCalibration.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Riccardo Musso
+/* Copyright (c) 2020-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, 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
@@ -23,93 +23,163 @@
 #pragma once
 
 #include <sensors/SensorData.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/Calibration.h>
+#include <sensors/calibration/SensorDataExtra.h>
 
 #include <Eigen/Core>
 
-#include "Calibration.h"
-#include "SensorDataExtra.h"
-
 namespace Boardcore
 {
 
 /**
- * This is the dumbest type of calibration possible: it stores a 3d vector
- * (called "bias") that will be added to every measurement.
- * During the calibration phase it will use a given reference vector (for
- * example the gravitational acceleration for the accelerometer), and every time
- * you'll feed the model with a new value, you have to give it the orientation
- * of the sensor, so it can guess the bias.
+ * @brief Applies an offset to the data
+ *
+ * It stores a vector that will be added to every measurement.
+ * Note: The type T must implement >> and << operators with a Vector3f.
+ *
+ * @tparam T Type used for data items.
  */
 template <typename T>
 class BiasCorrector : public ValuesCorrector<T>
 {
-
 public:
-    BiasCorrector() : bias(0, 0, 0) {}
-    BiasCorrector(const Eigen::Vector3f& _bias) : bias(_bias) {}
-
-    void operator>>(Eigen::Vector3f& rhs) { rhs = bias; }
+    BiasCorrector();
 
-    void operator<<(const Eigen::Vector3f& rhs) { bias = rhs; }
+    BiasCorrector(const Eigen::Vector3f& bias);
 
-    void setIdentity() override { bias = {0, 0, 0}; }
+    void operator>>(Eigen::Vector3f& rhs);
 
-    T correct(const T& data) const override
-    {
-        Eigen::Vector3f tmp;
-        T out;
+    void operator<<(const Eigen::Vector3f& rhs);
 
-        data >> tmp;
-        tmp += bias;
-        out << tmp;
+    void setIdentity() override;
 
-        return out;
-    }
+    T correct(const T& data) const override;
 
 private:
-    Eigen::Vector3f bias;
+    Eigen::Vector3f bias = Eigen::Vector3f::Zero();
 };
 
+/**
+ * @brief This is the dumbest type of calibration possible: an offset.
+ *
+ * During the calibration phase it will use a given reference vector (for
+ * example the gravitational acceleration), and every time you'll feed the model
+ * with a new value, you have to give it the orientation of the sensor, so it
+ * can guess the bias.
+ *
+ * @tparam T
+ */
 template <typename T>
 class BiasCalibration
-    : public AbstractCalibrationModel<T, BiasCorrector<T>, AxisOrientation>
+    : public AbstractCalibration<T, BiasCorrector<T>, AxisOrientation>
 {
 public:
-    BiasCalibration() : sum(0, 0, 0), ref(0, 0, 0), numSamples(0) {}
-
-    void setReferenceVector(Eigen::Vector3f vec) { ref = vec; }
-    Eigen::Vector3f getReferenceVector() { return ref; }
-
-    /**
-     * BiasCalibration accepts an indefinite number of samples,
-     * so feed(...) never returns false.
-     */
-    bool feed(const T& measured, const AxisOrientation& transform) override
-    {
-        Eigen::Vector3f vec;
-        measured >> vec;
-
-        sum += (transform.getMatrix().transpose() * ref) - vec;
-        numSamples++;
-
-        return true;
-    }
-
-    bool feed(const T& measured)
-    {
-        return feed(measured, AxisOrthoOrientation());
-    }
-
-    BiasCorrector<T> computeResult()
-    {
-        if (numSamples == 0)
-            return {Eigen::Vector3f{0, 0, 0}};
-        return {sum / numSamples};
-    }
+    BiasCalibration();
+
+    void setReferenceVector(Eigen::Vector3f vec);
+
+    Eigen::Vector3f getReferenceVector();
+
+    bool feed(const T& measured, const AxisOrientation& transform) override;
+
+    bool feed(const T& measured);
+
+    BiasCorrector<T> computeResult();
 
 private:
     Eigen::Vector3f sum, ref;
     unsigned numSamples;
 };
 
+template <typename T>
+BiasCorrector<T>::BiasCorrector()
+{
+}
+
+template <typename T>
+BiasCorrector<T>::BiasCorrector(const Eigen::Vector3f& bias) : bias(bias)
+{
+}
+
+template <typename T>
+void BiasCorrector<T>::operator>>(Eigen::Vector3f& rhs)
+{
+    rhs = bias;
+}
+
+template <typename T>
+void BiasCorrector<T>::operator<<(const Eigen::Vector3f& rhs)
+{
+    bias = rhs;
+}
+
+template <typename T>
+void BiasCorrector<T>::setIdentity()
+{
+    bias = {0, 0, 0};
+}
+
+template <typename T>
+T BiasCorrector<T>::correct(const T& data) const
+{
+    T output;
+    Eigen::Vector3f tmp;
+
+    data >> tmp;
+    tmp += bias;
+    output << tmp;
+
+    return output;
+}
+
+template <typename T>
+BiasCalibration<T>::BiasCalibration()
+    : sum(0, 0, 0), ref(0, 0, 0), numSamples(0)
+{
+}
+
+template <typename T>
+void BiasCalibration<T>::setReferenceVector(Eigen::Vector3f vec)
+{
+    ref = vec;
+}
+
+template <typename T>
+Eigen::Vector3f BiasCalibration<T>::getReferenceVector()
+{
+    return ref;
+}
+
+/**
+ * BiasCalibration accepts an indefinite number of samples,
+ * so feed(...) never returns false.
+ */
+template <typename T>
+bool BiasCalibration<T>::feed(const T& measured,
+                              const AxisOrientation& transform)
+{
+    Eigen::Vector3f vec;
+    measured >> vec;
+
+    sum += (transform.getMatrix().transpose() * ref) - vec;
+    numSamples++;
+
+    return true;
+}
+
+template <typename T>
+bool BiasCalibration<T>::feed(const T& measured)
+{
+    return feed(measured, AxisOrthoOrientation());
+}
+
+template <typename T>
+BiasCorrector<T> BiasCalibration<T>::computeResult()
+{
+    if (numSamples == 0)
+        return {Eigen::Vector3f{0, 0, 0}};
+    return {sum / numSamples};
+}
+
 }  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/Calibration.h b/src/shared/sensors/calibration/Calibration.h
index 8fa838df73d4de676d18c9256e4f9d86bb21caec..64673246b4ef425db93d3cddd2219d48533ee8da 100644
--- a/src/shared/sensors/calibration/Calibration.h
+++ b/src/shared/sensors/calibration/Calibration.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Riccardo Musso
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, 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
@@ -22,315 +22,67 @@
 
 #pragma once
 
-#include <sensors/Sensor.h>
-
 #include <Eigen/Core>
-#include <Eigen/Geometry>
 
 namespace Boardcore
 {
 
 /**
- * This class is used to adjust the values given by sensors during the flight.
- * An object can be obtained only via deserialization or if produced by an
- * instance of the "CalibrationModel" class.
- *
- * T is the type of sensor data the model is applied to (e.g.
- * GyroscopeData, MagnetometerData). This is needed because certain models could
- * work slightly differently depending on it
- *
- *  Note: derived classes of ValuesCorrector<XX> MUST implement the following
- *  operators (to make possible to store and load the coefficients):
+ * @brief This class is used to adjust the values given by a sensor.
  *
- *  operator << (const XX& t);
- *  operator >> (XX& t);
- *
- *  where XX is a datatype that can fully contain all the coefficients used
- *  by the function correct(input).
- *
- *  Also, an empty constructor must create a neutral instance (identity
- * transformation).
+ * @tparam T Type of sensor data the model is applied to (e.g.
+ * GyroscopeData, MagnetometerData).
  */
 template <typename T>
 class ValuesCorrector
 {
 public:
     /**
-     * This method will sets the internal coefficients so that the corrected
+     * @brief Sets the internal coefficients so that the corrected
      * values are exactly the same of the inputted ones.
      */
     virtual void setIdentity() = 0;
 
+    /**
+     * @brief Applies the correction to the given input.
+     */
     virtual T correct(const T& input) const = 0;
 };
 
 /**
- * AbstractCalibrationModel represents a "factory" of ValuesCorrector instances,
- * and it's necessary to create one. You will always use one of its derived
- * classes, of course.
+ * @brief Utility to compute calibration parameters for a specific sensor.
  *
- * Values given to the feed() function are needed for the training of the model.
+ * The calibration appens in two phases:
+ * - Sample collection: feed is called to store values;
+ * - Coefficient computation: computeResult is used to calculate the correction
+ * parameters.
  *
- * T is the sensor data type
- * C is a class that extends ValuesCorrector<T>
+ * @tparam T Sensor's data type used in the feed function.
+ * @tparam C ValuesCorrector returned by computeResult.
  */
 template <typename T, typename C, typename... AdditionalFeedParams>
-class AbstractCalibrationModel
+class AbstractCalibration
 {
 public:
     /**
-     * Gives to the model a single measurement to store and use to produce the
-     * adapted ValueCorrector instance
+     * @brief Stores the measurement for further processing.
      *
      * @returns false if the model can't accept the given data (usually because
-     * the internal buffers are full)
+     * the internal buffers are full).
      */
     virtual bool feed(const T& measurement,
                       const AdditionalFeedParams&... params) = 0;
 
     /**
-     * Creates the best ValuesCorrector instance for the given measurements.
-     * Note: you must feed some data to the model before getting the result!
+     * @brief Uses the recorded measurements to computer the correction
+     * parameters needed to correct sensor's data.
+     *
+     * Note: You need to feed the algorithm enough samples. Otherwise the method
+     * returns an ineffective corrector.
+     *
+     * @return ValuesCorrector containing the correction parameters.
      */
     virtual C computeResult() = 0;
-
-    virtual ~AbstractCalibrationModel(){};
-};
-
-/**
- * This class acts like a Sensor driver but incorporates both a Sensor<T>
- * instance and a ValuesCorrector. It can be useful to add a calibration step to
- * alredy existing code that uses the Sensor API.
- */
-template <typename SensorData>
-class SensorWrapper : public Sensor<SensorData>
-{
-    using S = Sensor<SensorData>;
-    using C = ValuesCorrector<SensorData>;
-
-public:
-    SensorWrapper(S* _sensor, C* _calib) : sensor(_sensor), calib(_calib) {}
-
-    S* getSensor() { return sensor; }
-    void setSensor(S* s) { sensor = s; }
-
-    C* getValuesCorrector() { return calib; }
-    void setValuesCorrector(C* c) { calib = c; }
-
-    void init() override { sensor->init(); }
-
-    bool test() override { return sensor->test(); }
-
-    SensorData sampleImpl() override
-    {
-        return calib->correct(sensor->sampleImpl());
-    }
-
-    SensorErrors getWrappedSensorError() { return sensor->getLastError(); }
-
-private:
-    S* sensor;
-    C* calib;
-};
-
-/**
- * This enum act like versors towards the chosen axis.
- *
- * X, Y and Z always set according to the right hand rule, so that:
- * X is the index
- * Y is the second finger
- * Z is the thumb
- *
- */
-enum class Direction : uint8_t
-{
-    POSITIVE_X = 0,
-    NEGATIVE_X,
-    POSITIVE_Y,
-    NEGATIVE_Y,
-    POSITIVE_Z,
-    NEGATIVE_Z,
-};
-
-constexpr const char* humanFriendlyDirection[]{
-    "North", "South", "East", "West", "Down", "Up",
-};
-
-inline Eigen::Vector3f orientationToVector(Direction val)
-{
-    switch (val)
-    {
-        case Direction::POSITIVE_X:
-            return {1, 0, 0};
-        case Direction::NEGATIVE_X:
-            return {-1, 0, 0};
-        case Direction::POSITIVE_Y:
-            return {0, 1, 0};
-        case Direction::NEGATIVE_Y:
-            return {0, -1, 0};
-        case Direction::POSITIVE_Z:
-            return {0, 0, 1};
-        case Direction::NEGATIVE_Z:
-            return {0, 0, -1};
-        default:
-            /* never happens, added just to shut up the warnings */
-            return {0, 0, 0};
-    }
-}
-
-/**
- * This struct represents in the most general way any kind of transformation of
- * the reference frame (axis X, Y and Z).
- * This data type is intended to simplify the code, so you shouldn't instantiate
- * this struct directly, but rather use the structures AxisAngleOrientation or
- * AxisOrthoOrientation that will be automatically corrected to this one, thanks
- * to the implicit cast in favor of AxisOrientation.
- *
- * For example:
- *
- * AxisAngleOrientation angles ( PI/2, PI, 0);
- * AxisOrthoOrientation ortho  ( Direction::NEGATIVE_X,
- * Direction::POSITIVE_Z );
- *
- * // The implicit cast is supported and recommended
- * AxisOrientation converted1 = angles;
- * AxisOrientation converted2 = ortho;
- *
- * // Now we can use the generated matrix:
- * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 }
- *
- */
-struct AxisOrientation
-{
-    Eigen::Matrix3f mat;
-
-    AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {}
-
-    AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {}
-
-    void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; }
-
-    Eigen::Matrix3f getMatrix() const { return mat; }
-};
-
-/**
- * This struct uses the three angles yaw, pitch and roll to define the
- * transformation of the reference frame, so according to N.E.D standard we get:
- *
- *         X (north)
- *        /
- *       /
- *      .----> Y (east)
- *      |
- *      |
- *      v
- *     Z (down)
- *
- * Where:
- * Yaw is rotation of Z axis
- * Pitch is rotation of Y axis
- * Roll is rotation of X axis
- */
-struct AxisAngleOrientation
-{
-    float yaw, pitch, roll;
-
-    AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {}
-
-    AxisAngleOrientation(float _yaw, float _pitch, float _roll)
-        : yaw(_yaw), pitch(_pitch), roll(_roll)
-    {
-    }
-
-    operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
-
-    Eigen::Matrix3f getMatrix() const
-    {
-        return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) *
-                Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) *
-                Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0}))
-            .toRotationMatrix();
-    }
-};
-
-/**
- * This struct represents the orientation of the reference frame relative
- * to X, Y, Z in the start orientation.
- * If we know the orientation of the X and Y axis, using the right hand rule
- * we can infer the Z axis.
- *
- * For example, if the base reference is:
- *
- *        z
- *        ^
- *        |
- *        |
- *        /----> y
- *       /
- *      x
- *
- * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get:
- *
- *          y   z
- *          ^   ^
- *          |  /
- *          | /
- *   x <----/
- *
- */
-struct AxisOrthoOrientation
-{
-    Direction xAxis, yAxis;
-
-    AxisOrthoOrientation()
-        : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y)
-    {
-    }
-
-    AxisOrthoOrientation(Direction _xAxis, Direction _yAxis)
-        : xAxis(_xAxis), yAxis(_yAxis)
-    {
-    }
-
-    operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
-
-    Eigen::Matrix3f getMatrix() const
-    {
-        Eigen::Vector3f vx, vy, vz;
-
-        vx = orientationToVector(xAxis);
-        vy = orientationToVector(yAxis);
-        vz = vx.cross(vy);
-
-        Eigen::Matrix3f mat;
-        mat.col(0) << vx;
-        mat.col(1) << vy;
-        mat.col(2) << vz;
-        return mat;
-    }
-};
-
-/**
- * This struct represents axis orientation relative to a reference system.
- * Operatively it simply combines two transformations. It is particularly useful
- * to obtain an AxisOrientation from a reference system generally not N.E.D.
- */
-struct AxisRelativeOrientation
-{
-    AxisOrientation systemOrientation, orientation;
-
-    AxisRelativeOrientation(const AxisOrientation& _systemOrientation,
-                            const AxisOrientation& _orientation)
-        : systemOrientation(_systemOrientation), orientation(_orientation)
-    {
-    }
-
-    operator AxisOrientation() const { return {getMatrix()}; }
-
-    Eigen::Matrix3f getMatrix() const
-    {
-        return systemOrientation.getMatrix() * orientation.getMatrix();
-    }
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/HardIronCalibration.h b/src/shared/sensors/calibration/HardIronCalibration.h
index 720f9a7edaae543870665af1e92f54ce9a4484ca..2cd616ac58379671d3958859f3eea3d12585ff64 100644
--- a/src/shared/sensors/calibration/HardIronCalibration.h
+++ b/src/shared/sensors/calibration/HardIronCalibration.h
@@ -68,7 +68,7 @@ private:
 
 template <unsigned MaxSamples>
 class HardIronCalibration
-    : public AbstractCalibrationModel<MagnetometerData, HardIronCorrector>
+    : public AbstractCalibration<MagnetometerData, HardIronCorrector>
 {
 public:
     HardIronCalibration() : samples(), numSamples(0) {}
diff --git a/src/shared/sensors/calibration/SensorDataExtra.cpp b/src/shared/sensors/calibration/SensorDataExtra.cpp
index f8ffd7ea87afd7c786ca4b59b98bf88453ad62c9..ed69028b1d522410bb381d021002bcd39bc0739c 100644
--- a/src/shared/sensors/calibration/SensorDataExtra.cpp
+++ b/src/shared/sensors/calibration/SensorDataExtra.cpp
@@ -34,6 +34,13 @@ void operator<<(AccelerometerData& lhs, const Vector3f& rhs)
     lhs.accelerationZ = rhs[2];
 }
 
+void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs)
+{
+    lhs[0] = rhs.accelerationX;
+    lhs[1] = rhs.accelerationY;
+    lhs[2] = rhs.accelerationZ;
+}
+
 void operator<<(GyroscopeData& lhs, const Vector3f& rhs)
 {
     lhs.angularVelocityX = rhs[0];
@@ -41,6 +48,13 @@ void operator<<(GyroscopeData& lhs, const Vector3f& rhs)
     lhs.angularVelocityZ = rhs[2];
 }
 
+void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs)
+{
+    lhs[0] = rhs.angularVelocityX;
+    lhs[1] = rhs.angularVelocityY;
+    lhs[2] = rhs.angularVelocityZ;
+}
+
 void operator<<(MagnetometerData& lhs, const Vector3f& rhs)
 {
     lhs.magneticFieldX = rhs[0];
@@ -48,25 +62,35 @@ void operator<<(MagnetometerData& lhs, const Vector3f& rhs)
     lhs.magneticFieldZ = rhs[2];
 }
 
-void operator>>(const AccelerometerData& lhs, Vector3f& rhs)
+void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs)
+{
+    lhs[0] = rhs.magneticFieldX;
+    lhs[1] = rhs.magneticFieldY;
+    lhs[2] = rhs.magneticFieldZ;
+}
+
+void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs)
 {
-    rhs[0] = lhs.accelerationX;
-    rhs[1] = lhs.accelerationY;
-    rhs[2] = lhs.accelerationZ;
+    rhs << lhs;
 }
 
-void operator>>(const GyroscopeData& lhs, Vector3f& rhs)
+void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs)
+{
+    rhs << lhs;
+}
+
+void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs) { rhs << lhs; }
+
+void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs) { rhs << lhs; }
+
+void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs)
 {
-    rhs[0] = lhs.angularVelocityX;
-    rhs[1] = lhs.angularVelocityY;
-    rhs[2] = lhs.angularVelocityZ;
+    rhs << lhs;
 }
 
-void operator>>(const MagnetometerData& lhs, Vector3f& rhs)
+void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs)
 {
-    rhs[0] = lhs.magneticFieldX;
-    rhs[1] = lhs.magneticFieldY;
-    rhs[2] = lhs.magneticFieldZ;
+    rhs << lhs;
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SensorDataExtra.h b/src/shared/sensors/calibration/SensorDataExtra.h
index 0885a3a5ead12f3eb67c718451031f4388a882f8..cc52c472f14c904ff56913c3bb7d8e54ef8f6bae 100644
--- a/src/shared/sensors/calibration/SensorDataExtra.h
+++ b/src/shared/sensors/calibration/SensorDataExtra.h
@@ -46,16 +46,26 @@ namespace Boardcore
 
 void operator<<(AccelerometerData& lhs, const Eigen::Vector3f& rhs);
 
+void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs);
+
 void operator<<(GyroscopeData& lhs, const Eigen::Vector3f& rhs);
 
+void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs);
+
 void operator<<(MagnetometerData& lhs, const Eigen::Vector3f& rhs);
 
+void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs);
+
 void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs);
 
+void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs);
+
 void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs);
 
-void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs);
+void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs);
 
 void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs);
 
+void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs);
+
 }  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SixParameterCalibration.h b/src/shared/sensors/calibration/SixParameterCalibration.h
index b080020861fb33ac622ad94047d82b4d71ba859d..8296a1ac974718cc02c0b174d1cd2c4f56a1482f 100644
--- a/src/shared/sensors/calibration/SixParameterCalibration.h
+++ b/src/shared/sensors/calibration/SixParameterCalibration.h
@@ -89,8 +89,7 @@ private:
 
 template <typename T, unsigned MaxSamples>
 class SixParameterCalibration
-    : public AbstractCalibrationModel<T, SixParameterCorrector<T>,
-                                      AxisOrientation>
+    : public AbstractCalibration<T, SixParameterCorrector<T>, AxisOrientation>
 {
 public:
     SixParameterCalibration() : samples(), ref(1, 0, 0), numSamples(0) {}
diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4d49d703425c8ec7d84fb1052e135b0b655ae97
--- /dev/null
+++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
@@ -0,0 +1,159 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, 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 "SoftAndHardIronCalibration.h"
+
+#include <sensors/calibration/SensorDataExtra.h>
+
+#include <iostream>
+
+using namespace Eigen;
+
+namespace Boardcore
+{
+
+SoftAndHardIronCorrector::SoftAndHardIronCorrector()
+    : SoftAndHardIronCorrector({0, 0, 0}, {1, 1, 1})
+{
+}
+
+SoftAndHardIronCorrector::SoftAndHardIronCorrector(const Vector3f& offset,
+                                                   const Vector3f& gain)
+    : offset(offset), gain(gain)
+{
+}
+
+void SoftAndHardIronCorrector::setIdentity()
+{
+    offset = {0, 0, 0};
+    gain   = {1, 1, 1};
+}
+
+MagnetometerData SoftAndHardIronCorrector::correct(
+    const MagnetometerData& sample) const
+{
+    MagnetometerData output;
+    Vector3f tmp;
+
+    tmp << sample;
+    tmp = (tmp + offset).cwiseProduct(gain);
+    output << tmp;
+
+    return output;
+}
+
+Eigen::Vector3f SoftAndHardIronCorrector::getOffset() const { return offset; }
+
+void SoftAndHardIronCorrector::setOffset(const Eigen::Vector3f& offset)
+{
+    this->offset = offset;
+}
+
+Eigen::Vector3f SoftAndHardIronCorrector::getGain() const { return gain; }
+
+void SoftAndHardIronCorrector::setGain(const Eigen::Vector3f& gain)
+{
+    this->gain = gain;
+}
+
+SoftAndHardIronCalibration::SoftAndHardIronCalibration() {}
+
+bool SoftAndHardIronCalibration::feed(const MagnetometerData& data)
+{
+    // Let S a matrix of Nx7 composed as [x^2, y^2, z^2, x, y, z, 1]
+    // D need to be S^T * S
+    // To avoid storing all measurements we just need to incrementally add to D
+
+    Vector3f vector;
+    vector << data;
+    Vector<float, 7> S;
+    // cppcheck-suppress constStatement
+    S << vector.cwiseProduct(vector), vector, 1;
+
+    for (int i = 0; i < 7; i++)
+        for (int j = 0; j < 7; j++)
+            D(i, j) += S(i) * S(j);
+
+    return true;
+}
+
+SoftAndHardIronCorrector SoftAndHardIronCalibration::computeResult()
+{
+    // Compute eigen value and vectors of D
+    SelfAdjointEigenSolver<Matrix<float, 7, 7>> solver(D);
+    auto eigenValues = solver.eigenvalues();
+
+    // Find the smallest eigen value and vector
+    float minValue = eigenValues[0];
+    int minIdx     = 0;
+
+    for (int i = 0; i < eigenValues.rows(); i++)
+        if (minValue > eigenValues[i])
+        {
+            minValue = eigenValues[i];
+            minIdx   = i;
+        }
+    Eigen::Matrix<float, 7, 1> vec = solver.eigenvectors().col(minIdx);
+
+    // Invert the vector if necessary
+    float det = vec[0] * vec[1] * vec[2];
+    if (det)
+    {
+        vec *= -1;
+        det *= -1;
+    }
+
+    // Compute offset and gain
+    Vector3f offset{vec[3] / vec[0] / 2, vec[4] / vec[1] / 2,
+                    vec[5] / vec[2] / 2};
+    Vector3f gain = (vec.block(0, 0, 3, 1) / cbrt(det)).cwiseSqrt();
+
+    return {offset, gain};
+}
+
+void operator<<(Eigen::Matrix<float, 3, 2>& lhs,
+                const SoftAndHardIronCorrector& rhs)
+{
+    lhs.col(0) = rhs.getOffset();
+    lhs.col(0) = rhs.getGain();
+}
+
+void operator<<(SoftAndHardIronCorrector& lhs,
+                const Eigen::Matrix<float, 3, 2>& rhs)
+{
+    lhs.setOffset(rhs.col(0));
+    lhs.setGain(rhs.col(1));
+}
+
+void operator>>(const Eigen::Matrix<float, 3, 2>& lhs,
+                SoftAndHardIronCorrector& rhs)
+{
+    rhs << lhs;
+}
+
+void operator>>(const SoftAndHardIronCorrector& lhs,
+                Eigen::Matrix<float, 3, 2>& rhs)
+{
+    rhs << lhs;
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..61b36c35fdcb91117a604a5f4cecc278ee5794f4
--- /dev/null
+++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h
@@ -0,0 +1,108 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, 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>
+#include <sensors/calibration/Calibration.h>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <vector>
+
+namespace Boardcore
+{
+
+/*
+ * The Soft-iron calibration removes the measurement error given by both the
+ * Hard and Soft Iron distortion of the magnetic field.
+ */
+class SoftAndHardIronCorrector : public ValuesCorrector<MagnetometerData>
+{
+public:
+    SoftAndHardIronCorrector();
+
+    SoftAndHardIronCorrector(const Eigen::Vector3f& offset,
+                             const Eigen::Vector3f& gain);
+
+    void setIdentity() override;
+
+    MagnetometerData correct(const MagnetometerData& sample) const override;
+
+    Eigen::Vector3f getOffset() const;
+
+    void setOffset(const Eigen::Vector3f& offset);
+
+    Eigen::Vector3f getGain() const;
+
+    void setGain(const Eigen::Vector3f& gain);
+
+private:
+    Eigen::Vector3f offset;
+    Eigen::Vector3f gain;
+};
+
+/**
+ * @brief Soft and hard iron calibration utility.
+ *
+ * Fits a non-rotated ellipsoid to the calibration data and then derives the
+ * correction parameters.
+ *
+ * Reference:
+ * https://www.st.com/resource/en/design_tip/dt0059-ellipsoid-or-sphere-fitting-for-sensor-calibration-stmicroelectronics.pdf
+ *
+ * @tparam MaxSamples
+ */
+class SoftAndHardIronCalibration
+    : public AbstractCalibration<MagnetometerData, SoftAndHardIronCorrector>
+{
+public:
+    SoftAndHardIronCalibration();
+
+    bool feed(const MagnetometerData& data) override;
+
+    /**
+     * @brief Uses the recorded measurements to compute the correction
+     * parameters needed to correct sensor's data.
+     *
+     * Note: Feed at leas 9 measurements!
+     *
+     * @return SoftAndHardIronCorrector containing the correction parameters.
+     */
+    SoftAndHardIronCorrector computeResult() override;
+
+private:
+    Eigen::Matrix<float, 7, 7> D = Eigen::Matrix<float, 7, 7>::Zero();
+};
+
+void operator<<(Eigen::Matrix<float, 3, 2>& lhs,
+                const SoftAndHardIronCorrector& rhs);
+
+void operator<<(SoftAndHardIronCorrector& lhs,
+                const Eigen::Matrix<float, 3, 2>& rhs);
+
+void operator>>(const Eigen::Matrix<float, 3, 2>& lhs,
+                SoftAndHardIronCorrector& rhs);
+
+void operator>>(const SoftAndHardIronCorrector& lhs,
+                Eigen::Matrix<float, 3, 2>& rhs);
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SoftIronCalibration.h b/src/shared/sensors/calibration/SoftIronCalibration.h
deleted file mode 100644
index 3b6dba4a9e4dc761798c71343191c7f68b994843..0000000000000000000000000000000000000000
--- a/src/shared/sensors/calibration/SoftIronCalibration.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Riccardo Musso
- *
- * 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>
-
-#include <Eigen/Core>
-#include <Eigen/Eigenvalues>
-
-#include "Calibration.h"
-
-namespace Boardcore
-{
-
-/*
- * The Soft-iron calibration removes the measurement error given by both the
- * Hard and Soft Iron distortion of the magnetic field.
- */
-class SoftIronCorrector : public ValuesCorrector<MagnetometerData>
-{
-public:
-    SoftIronCorrector() : SoftIronCorrector({1, 1, 1}, {0, 0, 0}) {}
-
-    SoftIronCorrector(const Eigen::Vector3f& _p, const Eigen::Vector3f& _q)
-        : p(_p), q(_q)
-    {
-    }
-
-    void setIdentity() override
-    {
-        p = {1, 1, 1};
-        q = {0, 0, 0};
-    }
-
-    void operator>>(Eigen::Matrix<float, 3, 2>& rhs)
-    {
-        rhs.col(0) = p.transpose();
-        rhs.col(1) = q.transpose();
-    }
-
-    void operator<<(const Eigen::Matrix<float, 3, 2>& rhs)
-    {
-        p = rhs.col(0).transpose();
-        q = rhs.col(1).transpose();
-    }
-
-    MagnetometerData correct(const MagnetometerData& input) const override
-    {
-        MagnetometerData output;
-        Eigen::Vector3f vec;
-
-        input >> vec;
-        vec = vec.cwiseProduct(p) + q;
-        output << vec;
-
-        return output;
-    }
-
-private:
-    Eigen::Vector3f p, q;
-};
-
-template <unsigned MaxSamples>
-class SoftIronCalibration
-    : public AbstractCalibrationModel<MagnetometerData, SoftIronCorrector>
-{
-public:
-    SoftIronCalibration() : samples(), numSamples(0) {}
-
-    bool feed(const MagnetometerData& data) override
-    {
-        if (numSamples >= MaxSamples)
-            return false;
-
-        Eigen::Vector3f vec;
-        data >> vec;
-
-        for (int i = 0; i < 3; ++i)
-        {
-            samples(numSamples, i)     = vec[i] * vec[i];
-            samples(numSamples, i + 3) = vec[i];
-        }
-        samples(numSamples, 6) = 1;
-
-        numSamples++;
-        return true;
-    }
-
-    SoftIronCorrector computeResult() override
-    {
-        using Mx   = Eigen::Matrix<float, 7, 7>;
-        using Vec7 = Eigen::Matrix<float, 7, 1>;
-
-        float minValue, det;
-        int minIdx;
-
-        Eigen::MatrixXf tmp1, tmp2;
-        Eigen::Vector3f p, q;
-        Vec7 vec;
-        Mx mat;
-
-        tmp1 = samples.block(0, 0, numSamples, 7);
-        tmp2 = tmp1.transpose();
-
-        // mat = tmp2 * tmp1;
-
-        /*
-         * tmp1: N x 7
-         * tmp2: 7 x N
-         *
-         * Big matrices multiplication: if done with Eigen, the program crashes
-         * (bus fault), so we'll do that with good old for's
-         */
-        for (unsigned i = 0; i < 7; ++i)
-        {
-            for (unsigned j = 0; j < 7; ++j)
-            {
-                mat(i, j) = 0;
-
-                for (unsigned k = 0; k < numSamples; ++k)
-                {
-                    mat(i, j) += tmp2(i, k) * tmp1(k, j);
-                }
-            }
-        }
-
-        Eigen::SelfAdjointEigenSolver<Mx> solver(mat);
-        auto eigenvalues = solver.eigenvalues();
-
-        minValue = eigenvalues[0];
-        minIdx   = 0;
-
-        for (int i = 1; i < eigenvalues.rows(); ++i)
-        {
-            if (minValue > eigenvalues[i])
-            {
-                minValue = eigenvalues[i];
-                minIdx   = i;
-            }
-        }
-
-        vec = solver.eigenvectors().col(minIdx);
-        det = vec[0] * vec[1] * vec[2];
-
-        if (det < 0)
-        {
-            vec *= -1;
-            det *= -1;
-        }
-
-        p = vec.block(0, 0, 3, 1) / cbrt(det);
-        q = {vec[3] / vec[0] / 2, vec[4] / vec[1] / 2, vec[5] / vec[2] / 2};
-
-        p[0] = sqrt(p[0]);
-        p[1] = sqrt(p[1]);
-        p[2] = sqrt(p[2]);
-
-        q = q.cwiseProduct(p);
-
-        return {p, q};
-    }
-
-    Eigen::Matrix<float, MaxSamples, 7> getSamples() { return samples; }
-
-private:
-    /*
-     * The matrix contains x, y, z, x^2, y^2, z^2 and a column of 1s
-     * row. Its shape is (N x 7)
-     */
-    Eigen::Matrix<float, MaxSamples, 7> samples;
-    unsigned numSamples;
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/sensors/calibration/TwelveParameterCalibration.h b/src/shared/sensors/calibration/TwelveParameterCalibration.h
index 061fbbb25d61f7e76a4f1091a32f17aa00f10b9a..832eac3d4466512f365aeee270d0946915f7d184 100644
--- a/src/shared/sensors/calibration/TwelveParameterCalibration.h
+++ b/src/shared/sensors/calibration/TwelveParameterCalibration.h
@@ -97,8 +97,8 @@ private:
 
 template <typename T, unsigned MaxSamples>
 class TwelveParameterCalibration
-    : public AbstractCalibrationModel<T, TwelveParameterCorrector<T>,
-                                      AxisOrientation>
+    : public AbstractCalibration<T, TwelveParameterCorrector<T>,
+                                 AxisOrientation>
 {
 public:
     TwelveParameterCalibration() : samples(), numSamples(0), ref({1, 0, 0}) {}
diff --git a/src/shared/utils/aero/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp
similarity index 76%
rename from src/shared/utils/aero/AeroUtils.cpp
rename to src/shared/utils/AeroUtils/AeroUtils.cpp
index 434236c180f0f7fe4bdfd2a55330d5ecab9887c9..c984cdfc247550036c4d88eef544fc4cf4d68663 100644
--- a/src/shared/utils/aero/AeroUtils.cpp
+++ b/src/shared/utils/AeroUtils/AeroUtils.cpp
@@ -22,15 +22,19 @@
 
 #include "AeroUtils.h"
 
+#include <utils/Constants.h>
+
+using namespace Eigen;
+
 namespace Boardcore
 {
 
-namespace aeroutils
+namespace Aeroutils
 {
 
 float relAltitude(float pressure, float pressureRef, float temperatureRef)
 {
-    using namespace constants;
+    using namespace Constants;
 
     return temperatureRef / a * (1 - powf(pressure / pressureRef, nInv));
 }
@@ -38,7 +42,7 @@ float relAltitude(float pressure, float pressureRef, float temperatureRef)
 float relDensity(float pressure, float pressureRef, float altitudeRef,
                  float temperatureRef)
 {
-    using namespace constants;
+    using namespace Constants;
 
     return pressure / (R * a * altitudeRef +
                        R * temperatureRef * powf(pressure / pressureRef, nInv));
@@ -46,7 +50,7 @@ float relDensity(float pressure, float pressureRef, float altitudeRef,
 
 float mslPressure(float pressureRef, float temperatureRef, float altitudeRef)
 {
-    using namespace constants;
+    using namespace Constants;
     float T0 = mslTemperature(temperatureRef, altitudeRef);
 
     return pressureRef / powf(1 - a * altitudeRef / T0, n);
@@ -54,16 +58,28 @@ float mslPressure(float pressureRef, float temperatureRef, float altitudeRef)
 
 float mslTemperature(float temperatureRef, float altitudeRef)
 {
-    return temperatureRef + (altitudeRef * constants::a);
+    return temperatureRef + (altitudeRef * Constants::a);
 }
 
 float verticalSpeed(float p, float dpDt, float pRef, float tRef)
 {
-    using namespace constants;
+    using namespace Constants;
 
     return -(tRef * dpDt * powf(p / pRef, nInv)) / (a * n * p);
 }
 
-}  // namespace aeroutils
+Vector2f geodetic2NED(const Vector2f& gpsData, const Vector2f& offset)
+{
+    float mPerDegLat = 111132.95225;
+    float mPerDegLon =
+        fabsf(111412.87733 * cosf(gpsData[0] * Constants::DEGREES_TO_RADIANS));
+
+    return {
+        mPerDegLat * (gpsData[0] - offset[0]),
+        mPerDegLon * (gpsData[1] - offset[1]),
+    };
+}
+
+}  // namespace Aeroutils
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/aero/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
similarity index 73%
rename from src/shared/utils/aero/AeroUtils.h
rename to src/shared/utils/AeroUtils/AeroUtils.h
index 24f0d3edde0b4cf1a22a78410941decde2398e7a..80abb8dac650fc6054e359b34e65442bf1402d41 100644
--- a/src/shared/utils/aero/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -22,34 +22,18 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <cmath>
 
 namespace Boardcore
 {
 
-namespace aeroutils
+namespace Aeroutils
 {
 
-namespace constants
-{
-
-// Troposphere temperature gradient [deg/m]
-constexpr float a = 0.0065f;
-
-// Acceleration of gravity [m^s^2]
-constexpr float g = 9.80665f;
-
-// Air gas constant [J/(Kg*K])]
-constexpr float R = 287.05f;
-
-constexpr float n    = g / (R * a);
-constexpr float nInv = (R * a) / g;
-
-}  // namespace constants
-
 /**
  * Returns the current altitude with respect to a reference altitude for the
- * given pressure, using International Standard Atmosphere  model.
+ * given pressure, using International Standard Atmosphere model.
  *
  * @warning This function is valid for altitudes below 11000 meters above sea
  * level
@@ -57,10 +41,10 @@ constexpr float nInv = (R * a) / g;
  * altitude. It does not provide altitude above mean sea level unless the
  * reference is, in fact, the sea level.
  *
- * @param pressure Current pressure [Pascal]
- * @param pressureRef Pressure at reference altitude (must be > 0) [Pascal]
- * @param temperatureRef Temperature at reference altitude [Kelvin]
- * @return Current altitude with respect to the reference altitude [meters]
+ * @param pressure Current pressure [Pas]
+ * @param pressureRef Pressure at reference altitude (must be > 0) [Pa]
+ * @param temperatureRef Temperature at reference altitude [K]
+ * @return Current altitude with respect to the reference altitude [m]
  */
 float relAltitude(float pressure, float pressureRef, float temperatureRef);
 
@@ -68,10 +52,10 @@ float relAltitude(float pressure, float pressureRef, float temperatureRef);
  * Returns the current air density with respect to a reference density and
  * temperature, using the Internation Standard Atmosphere model.
  *
- * @param pressure Current atmospheric pressure [Pascal]
- * @param pressureRef Pressure at reference altitude (must be > 0) [Pascal]
+ * @param pressure Current atmospheric pressure [Pa]
+ * @param pressureRef Pressure at reference altitude (must be > 0) [Pa]
  * @param altitudeRef Reference altitude [m]
- * @param temperatureRef Temperature at reference altitude [Kelvin]
+ * @param temperatureRef Temperature at reference altitude [K]
  * @return Current air density  [Kg/m^3]
  */
 float relDensity(float pressure, float pressureRef, float altitudeRef,
@@ -85,11 +69,10 @@ float relDensity(float pressure, float pressureRef, float altitudeRef,
  * @warning This function is valid for altitudes below 11000 meters above sea
  * level
  *
- * @param pressureRef Pressure at reference altitude [Pascal]
- * @param temperatureRef Temperature at reference altitude. Must be > 0
- * [Kelvin]
- * @param altitudeRef Reference altitude [meters]
- * @return Pressure at mean sea level [pascal]
+ * @param pressureRef Pressure at reference altitude [Pa]
+ * @param temperatureRef Temperature at reference altitude. Must be > 0 [K]
+ * @param altitudeRef Reference altitude [m]
+ * @return Pressure at mean sea level [Pa]
  */
 float mslPressure(float pressureRef, float temperatureRef, float altitudeRef);
 
@@ -100,9 +83,9 @@ float mslPressure(float pressureRef, float temperatureRef, float altitudeRef);
  * @warning This function is valid for altitudes below 11000 meters above sea
  * level
  *
- * @param temperatureRef Temperature at reference altitude [Kelvin]
- * @param altitudeRef Reference altitude [meters]
- * @return Temperature at mean sea level [Kelvin]
+ * @param temperatureRef Temperature at reference altitude [K]
+ * @param altitudeRef Reference altitude [m]
+ * @return Temperature at mean sea level [K]
  */
 float mslTemperature(float temperatureRef, float altitudeRef);
 
@@ -114,13 +97,24 @@ float mslTemperature(float temperatureRef, float altitudeRef);
  * level
  *
  * @param p Current pressure (must be > 0) [Pa]
- * @param dpDt [Rate of change of pressure [Pa/s]]
+ * @param dpDt Rate of change of pressure [Pa/s]
  * @param pRef Reference pressure (must be > 0) [Pa]
  * @param tRef Reference temperature [K]
  * @return Vertical speed, positive upwards [m/s]
  */
 float verticalSpeed(float p, float dpDt, float pRef, float tRef);
 
-}  // namespace aeroutils
+/**
+ * @brief Converts decimal degrees of latitude and longitude into displacement
+ * in meters between two positions the with an ellipsoidal earth model.
+ *
+ * @param position1 Latitude and longitude of current position [lat lon][deg]
+ * @param position2 Initial position used as an offset [lat lon][deg]
+ * @return Distance between the two coordinates [n e][m]
+ */
+Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& position1,
+                             const Eigen::Vector2f& position2);
+
+}  // namespace Aeroutils
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/ButtonHandler.h b/src/shared/utils/ButtonHandler.h
deleted file mode 100644
index c803f4201d12e87b05a4e859364e397abfe4a3ed..0000000000000000000000000000000000000000
--- a/src/shared/utils/ButtonHandler.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/* Copyright (c) 2015-2018 Skyward Experimental Rocketry
- * Author: Luca Erbetta
- *
- * 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 <diagnostic/PrintLogger.h>
-
-#include <functional>
-
-#include "ActiveObject.h"
-
-using miosix::Thread;
-
-namespace Boardcore
-{
-
-static const int TICK_LENGTH           = 100;  // milliseconds
-static const int LONG_PRESS_TICKS      = 10;
-static const int VERY_LONG_PRESS_TICKS = 50;
-
-enum class ButtonPress
-{
-    DOWN,      // Called as soon as the button is pressed
-    UP,        // Called when the button is released
-    SHORT,     // Short press
-    LONG,      // Long press
-    VERY_LONG  // Very long press
-};
-
-/**
- * @brief Class that detects if a button is pressed, long pressed or long-long
- * pressed and calls a callback in each case
- *
- * @tparam Button GPIO of the button
- */
-template <typename Button>
-class ButtonHandler : public ActiveObject
-{
-public:
-    using ButtonCallback = std::function<void(uint8_t, ButtonPress)>;
-
-    static bool isPressed() { return Button::value(); }
-
-    ButtonHandler(uint8_t btnId, ButtonCallback callback)
-        : btnId(btnId), callback(callback)
-    {
-        Button::mode(miosix::Mode::INPUT);
-    }
-
-    ~ButtonHandler() {}
-
-protected:
-    void run() override
-    {
-        while (!shouldStop() && callback)
-        {
-            if (Button::value())
-            {
-                if (!pressed && callback)
-                {
-                    callback(btnId, ButtonPress::DOWN);
-                }
-
-                pressedTicks++;
-                pressed = true;
-            }
-            else if (pressed)  // if the button was unpressed since
-                               // the last operation
-            {
-                if (pressedTicks >= VERY_LONG_PRESS_TICKS)
-                {
-                    LOG_DEBUG(logger, "Button pressed (very long) (%d ticks)",
-                              pressedTicks);
-
-                    callback(btnId, ButtonPress::VERY_LONG);
-                }
-                else if (pressedTicks >= LONG_PRESS_TICKS)
-                {
-                    LOG_DEBUG(logger, "Button pressed (long) (%d ticks)",
-                              pressedTicks);
-                    callback(btnId, ButtonPress::LONG);
-                }
-                else
-                {
-                    LOG_DEBUG(logger, "Button pressed (short) (%d ticks)",
-                              pressedTicks);
-                    callback(btnId, ButtonPress::SHORT);
-                }
-
-                callback(btnId, ButtonPress::UP);
-
-                pressed      = false;
-                pressedTicks = 0;
-            }
-
-            Thread::sleep(TICK_LENGTH);
-        }
-    }
-
-private:
-    uint8_t btnId    = 0;
-    bool pressed     = false;
-    int pressedTicks = 0;
-
-    ButtonCallback callback;
-
-    PrintLogger logger = Logging::getLogger("buttonhandler");
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/utils/ButtonHandler/ButtonHandler.cpp b/src/shared/utils/ButtonHandler/ButtonHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56a0e5df40a5f5e5b2d12d4c3c80e2f0bfb00774
--- /dev/null
+++ b/src/shared/utils/ButtonHandler/ButtonHandler.cpp
@@ -0,0 +1,100 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "ButtonHandler.h"
+
+#include <functional>
+
+namespace Boardcore
+{
+
+bool ButtonHandler::registerButtonCallback(miosix::GpioPin pin,
+                                           ButtonCallback callback)
+{
+    // Try to insert the callback
+    auto result = callbacks.insert({pin, {callback, false, 0}});
+
+    // Check if the insertion took place
+    if (result.second)
+    {
+        return scheduler.addTask(
+            std::bind(&ButtonHandler::periodicButtonValueCheck, this, pin),
+            SAMPLE_PERIOD, TaskScheduler::Policy::SKIP);
+    }
+
+    return false;
+}
+
+bool ButtonHandler::start() { return scheduler.start(); }
+
+void ButtonHandler::stop() { scheduler.stop(); }
+
+ButtonHandler::ButtonHandler()
+{
+    // Start the scheduler immediately
+    scheduler.start();
+}
+
+void ButtonHandler::periodicButtonValueCheck(miosix::GpioPin pin)
+{
+    // Make sure the pin informations are still present
+    if (callbacks.find(pin) == callbacks.end())
+        return;
+
+    // Retrieve the pin information
+    const ButtonCallback &callback = std::get<0>(callbacks[pin]);
+    bool &wasPressed               = std::get<1>(callbacks[pin]);
+    int &pressedTicks              = std::get<2>(callbacks[pin]);
+
+    // Read the current button status
+    // Note: The button is assumed to be pressed if the pin value is low
+    // (pulldown)
+    bool isNowPressed = !pin.value();
+
+    if (isNowPressed)
+    {
+        // If the pin was not pressed before it has just been pressed
+        if (!wasPressed && callback)
+            callback(ButtonEvent::PRESSED);
+
+        // Increment the tick
+        pressedTicks++;
+    }
+    // If the button was pressed before and just released
+    else if (wasPressed)
+    {
+        if (pressedTicks >= VERY_LONG_PRESS_TICKS)
+            callback(ButtonEvent::VERY_LONG_PRESS);
+        else if (pressedTicks >= LONG_PRESS_TICKS)
+            callback(ButtonEvent::LONG_PRESS);
+        else
+            callback(ButtonEvent::SHORT_PRESS);
+
+        // Reset the ticks
+        pressedTicks = 0;
+    }
+
+    // Save the current button status
+    wasPressed = isNowPressed;
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/utils/ButtonHandler/ButtonHandler.h b/src/shared/utils/ButtonHandler/ButtonHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ff97a9beb9e4acb7e6e98c45ee88550fa3deddf
--- /dev/null
+++ b/src/shared/utils/ButtonHandler/ButtonHandler.h
@@ -0,0 +1,140 @@
+/* Copyright (c) 2015-2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Singleton.h>
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <map>
+
+namespace Boardcore
+{
+
+enum class ButtonEvent
+{
+    PRESSED,         // Called as soon as the button is pressed
+    SHORT_PRESS,     // Called as soon as the button is released
+    LONG_PRESS,      // Called as soon as the button is released
+    VERY_LONG_PRESS  // Called as soon as the button is released
+};
+
+/**
+ * @brief Comparison operator between GpioPins used for std::map.
+ */
+struct GpioPinCompare
+{
+    bool operator()(const miosix::GpioPin& lhs,
+                    const miosix::GpioPin& rhs) const
+    {
+        if (lhs.getPort() == rhs.getPort())
+            return lhs.getNumber() < rhs.getNumber();
+        return lhs.getPort() < rhs.getPort();
+    }
+};
+
+/**
+ * @brief Utility to detects if buttons are pressed, long pressed or long-long
+ * pressed and calls a callback in each case
+ *
+ * Note: The ButtonHandler assumes the all the buttons to be pulldown meaning
+ * that when the button is pressed, the pin is assumed low.
+ *
+ * TODO: Allow to set pullup or pulldown configuration for each registered pin.
+ */
+class ButtonHandler : public Singleton<ButtonHandler>
+{
+    friend Singleton<ButtonHandler>;
+
+    static constexpr uint32_t SAMPLE_PERIOD    = 100;  // 10Hz
+    static constexpr int LONG_PRESS_TICKS      = 10;   // 1s
+    static constexpr int VERY_LONG_PRESS_TICKS = 50;   // 5s
+
+public:
+    using ButtonCallback = std::function<void(ButtonEvent)>;
+
+    /**
+     * @brief Registers a callback on the specified pin.
+     *
+     * @param pin Pin to listen to.
+     * @param callback Function to call on button events.
+     * @return False if another callback was already registered for the pin.
+     */
+    bool registerButtonCallback(miosix::GpioPin pin, ButtonCallback callback);
+
+    /**
+     * @brief Unregisters the callback associated with the specified pin, if
+     * any.
+     *
+     * @param pin Pin whose callback function is to be removed.
+     * @return True if a callback was present and removed for the given pin.
+     */
+    bool unregisterButtonCallback(miosix::GpioPin pin);
+
+    /**
+     * @brief Starts the ButtonHandler's task scheduler.
+     *
+     * Note that the scheduler is started as soon as the ButtonHandler is first
+     * used.
+     *
+     * @return Whether the task scheduler was started or not.
+     */
+    bool start();
+
+    /**
+     * @brief Stops the ButtonHandler's task scheduler.
+     */
+    void stop();
+
+private:
+    ButtonHandler();
+
+    /**
+     * @brief This function is added to the scheduler for every pin registered
+     * in the ButtonHandler.
+     *
+     * @param pin Pin whose value need to be checked.
+     */
+    void periodicButtonValueCheck(miosix::GpioPin pin);
+
+    TaskScheduler scheduler;
+
+    /**
+     * @brief Map of all the callbacks registered in the ButtonHandler.
+     *
+     * The key is the GpioPin for which the callback is registered. To used
+     * GpioPin as a map key, the GpioPinCompare operator was defined as
+     * explained here:
+     * https://stackoverflow.com/questions/1102392/how-can-i-use-stdmaps-with-user-defined-types-as-key
+     *
+     * The type stored is a tuple containing:
+     * - The button callback function;
+     * - Whether or not the button was pressed in the last check iteration;
+     * - The relative tick of the last pin value change.
+     */
+    std::map<miosix::GpioPin, std::tuple<ButtonCallback, bool, int>,
+             GpioPinCompare>
+        callbacks;
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/utils/CSVReader/CSVReader.h b/src/shared/utils/CSVReader/CSVReader.h
index 32388292c5890d88e3fa43182e049ac89ddaa477..a8f59a5efae736e8de7fb31193163d7fb4964f16 100644
--- a/src/shared/utils/CSVReader/CSVReader.h
+++ b/src/shared/utils/CSVReader/CSVReader.h
@@ -123,6 +123,9 @@ private:
 /**
  * @brief Iterable parser of CSV files.
  *
+ * If the CSV file has an header row, you must specify true as the second
+ * parameter in the constructor.
+ *
  * Given the file name, reads the contents as elements of type Data. Can be used
  * with CSVIterator to iterate through all the CSV rows.
  * You can retrieve all data inside the file as a vector with collect().
@@ -134,7 +137,14 @@ template <typename Data>
 class CSVParser
 {
 public:
-    CSVParser(const char* fileName) : fileStream(fileName) {}
+    CSVParser(const char* fileName, bool hasHeader = false)
+        : fileStream(fileName)
+    {
+        // If the file has the header, ignore everithing in the first line
+        if (hasHeader)
+            fileStream.ignore(std::numeric_limits<std::streamsize>::max(),
+                              '\n');
+    }
 
     ~CSVParser() { fileStream.close(); }
 
diff --git a/src/shared/utils/ClockUtils.h b/src/shared/utils/ClockUtils.h
index 6e7c449b53e0cf8a2e0098fce331c331f900cb4c..3d671d9f48311aae9bae40d24b5f022e6d9d8e8e 100644
--- a/src/shared/utils/ClockUtils.h
+++ b/src/shared/utils/ClockUtils.h
@@ -341,6 +341,8 @@ inline bool ClockUtils::enablePeripheralClock(void* peripheral)
             return false;
     }
 
+    RCC_SYNC();
+
     return true;
 }
 
@@ -575,6 +577,8 @@ inline bool ClockUtils::disablePeripheralClock(void* peripheral)
             return false;
     }
 
+    RCC_SYNC();
+
     return true;
 }
 
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index a2f8ca35922c623794e83229466daa013344b856..ddc1e810901159f6b92d95041a8a3606dd8ef939 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -25,13 +25,23 @@
 namespace Boardcore
 {
 
-static constexpr const float PI                 = 3.14159265f;
-static constexpr const float EARTH_GRAVITY      = 9.80665f;
-static constexpr const float EARTH_RADIUS       = 6371.0f * 1000.0f;  // [m]
+namespace Constants
+{
+
+static constexpr const float PI                 = 3.14159265f;  // [rad]
 static constexpr const float DEGREES_TO_RADIANS = PI / 180.0f;
 static constexpr const float RADIANS_TO_DEGREES = 180.0f / PI;
-static constexpr const float KNOTS_TO_MPS       = 0.514444;
-static constexpr const float MSL_PRESSURE       = 101325.0f;  // [Pa]
-static constexpr const float MSL_TEMPERATURE    = 288.15f;    // [Kelvin]
+
+static constexpr const float g = 9.80665f;  // [m^s^2]
+
+static constexpr float a = 0.0065f;  // Troposphere temperature gradient [deg/m]
+static constexpr float R = 287.05f;  // Air gas constant [J/Kg/K]
+static constexpr float n = g / (R * a);
+static constexpr float nInv = (R * a) / g;
+
+static constexpr const float MSL_PRESSURE    = 101325.0f;  // [Pa]
+static constexpr const float MSL_TEMPERATURE = 288.15f;    // [Kelvin]
+
+}  // namespace Constants
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/MovingAverage.h b/src/shared/utils/MovingAverage.h
index 94e8fda20d5577c21c9de585a6f3081547c2a0e3..6a6efd1bbcf6790efa89a17919146cb6bc770b37 100644
--- a/src/shared/utils/MovingAverage.h
+++ b/src/shared/utils/MovingAverage.h
@@ -45,12 +45,21 @@ public:
         value += newValue * movingAverageCoeff;
     }
 
+    void reset() { value = 0; }
+
+    void setN(int movingAverageN)
+    {
+        this->movingAverageN   = movingAverageN;
+        movingAverageCoeff     = 1 / (float)movingAverageN;
+        movingAverageCompCoeff = 1 - movingAverageCoeff;
+    }
+
 private:
     T value = 0;
 
-    const int movingAverageN           = 20;
-    const float movingAverageCoeff     = 1 / (float)movingAverageN;
-    const float movingAverageCompCoeff = 1 - movingAverageCoeff;
+    int movingAverageN           = 20;
+    float movingAverageCoeff     = 1 / (float)movingAverageN;
+    float movingAverageCompCoeff = 1 - movingAverageCoeff;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/math/SkyQuaternion.cpp b/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
similarity index 51%
rename from src/shared/math/SkyQuaternion.cpp
rename to src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
index d8f678341d333d84a30f2bdf127ecf8b57a2b79c..1af74a6b622c2c287b4e602167ac5d0d0e1335af 100644
--- a/src/shared/math/SkyQuaternion.cpp
+++ b/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Marco Cella
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Marco Cella, 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
@@ -32,13 +32,15 @@ using namespace Eigen;
 namespace Boardcore
 {
 
-SkyQuaternion::SkyQuaternion() {}
+namespace SkyQuaternion
+{
 
-Vector4f SkyQuaternion::eul2quat(Vector3f degeul)  // ZYX rotation
+Vector4f eul2quat(Vector3f euler)
 {
-    float yaw   = degeul(0) * DEGREES_TO_RADIANS;
-    float pitch = degeul(1) * DEGREES_TO_RADIANS;
-    float roll  = degeul(2) * DEGREES_TO_RADIANS;
+    // Euler angles are ZYX
+    float yaw   = euler(0) * Constants::DEGREES_TO_RADIANS;
+    float pitch = euler(1) * Constants::DEGREES_TO_RADIANS;
+    float roll  = euler(2) * Constants::DEGREES_TO_RADIANS;
 
     float cyaw   = cosf(yaw * 0.5F);
     float syaw   = sinf(yaw * 0.5F);
@@ -52,52 +54,45 @@ Vector4f SkyQuaternion::eul2quat(Vector3f degeul)  // ZYX rotation
     float qz = syaw * cpitch * croll - cyaw * spitch * sroll;
     float qw = cyaw * cpitch * croll + syaw * spitch * sroll;
 
-    Vector4f quat(qx, qy, qz, qw);
-
-    return quat;
+    return Vector4f(qx, qy, qz, qw);
 }
 
-Vector3f SkyQuaternion::quat2eul(Vector4f quater)
+Vector3f quat2eul(Vector4f quat)
 {
-    float qx = quater(0);
-    float qy = quater(1);
-    float qz = quater(2);
-    float qw = quater(3);
+    // Euler angles are ZYX
+    float qx = quat(0);
+    float qy = quat(1);
+    float qz = quat(2);
+    float qw = quat(3);
 
     float yaw = atan2f(2.0F * (qx * qy + qw * qz),
                        powf(qw, 2) + powf(qx, 2) - powf(qy, 2) - powf(qz, 2));
 
     float a = -2.0F * (qx * qz - qw * qy);
     if (a > 1.0F)
-    {
         a = 1.0F;
-    }
     else if (a < -1.0F)
-    {
         a = -1.0F;
-    }
 
     float pitch = asinf(a);
 
     float roll = atan2f(2.0F * (qy * qz + qw * qx),
                         powf(qw, 2) - powf(qx, 2) - powf(qy, 2) + powf(qz, 2));
 
-    Vector3f eul(roll, pitch, yaw);
-
-    return eul * 180.0F / PI;
+    return Vector3f(roll, pitch, yaw) * Constants::RADIANS_TO_DEGREES;
 }
 
-Vector4f SkyQuaternion::rotm2quat(Matrix3f R)
+Vector4f rotationMatrix2quat(Matrix3f rtm)
 {
-    float r11 = R(0, 0);
-    float r12 = R(0, 1);
-    float r13 = R(0, 2);
-    float r21 = R(1, 0);
-    float r22 = R(1, 1);
-    float r23 = R(1, 2);
-    float r31 = R(2, 0);
-    float r32 = R(2, 1);
-    float r33 = R(2, 2);
+    float r11 = rtm(0, 0);
+    float r12 = rtm(0, 1);
+    float r13 = rtm(0, 2);
+    float r21 = rtm(1, 0);
+    float r22 = rtm(1, 1);
+    float r23 = rtm(1, 2);
+    float r31 = rtm(2, 0);
+    float r32 = rtm(2, 1);
+    float r33 = rtm(2, 2);
 
     float qx;
     float qy;
@@ -105,63 +100,63 @@ Vector4f SkyQuaternion::rotm2quat(Matrix3f R)
     float qw;
 
     float tr = r11 + r22 + r33;
-    float r  = sqrt(1 + tr);
 
-    if (r32 - r23 > 0)
-    {
-        qx = 0.5 * sqrt(1 + r11 - r22 - r33);
-    }
-    else if (r32 - r23 < 0)
+    if (tr > 0)
     {
-        qx = -0.5 * sqrt(1 + r11 - r22 - r33);
-    }
-    else
-    {
-        qx = 0;
-    }
+        float sqtrp1 = sqrt(1 + tr);
 
-    if (r13 - r31 > 0)
-    {
-        qy = 0.5 * sqrt(1 - r11 + r22 - r33);
-    }
-    else if (r13 - r31 < 0)
-    {
-        qy = -0.5 * sqrt(1 - r11 + r22 - r33);
+        qx = (r23 - r32) / (2.0 * sqtrp1);
+        qy = (r31 - r13) / (2.0 * sqtrp1);
+        qz = (r12 - r21) / (2.0 * sqtrp1);
+        qw = 0.5 * sqtrp1;
     }
     else
     {
-        qy = 0;
-    }
+        if ((r22 > r11) && (r22 > r33))
+        {
+            float sqdip1 = sqrt(r22 - r11 - r33 + 1.0);
 
-    if (r21 - r12 > 0)
-    {
-        qz = 0.5 * sqrt(1 - r11 - r22 + r33);
-    }
-    else if (r21 - r12 < 0)
-    {
-        qz = -0.5 * sqrt(1 - r11 - r22 + r33);
-    }
-    else
-    {
-        qz = 0;
-    }
+            qy = 0.5 * sqdip1;
 
-    qw = 0.5 * r;
+            if (sqdip1 != 0)
+                sqdip1 = 0.5 / sqdip1;
 
-    if (qw < 0)
-    {
-        qx = -qx;
-        qy = -qy;
-        qz = -qz;
-        qw = -qw;
-    }
+            qx = (r12 + r21) * sqdip1;
+            qz = (r23 + r32) * sqdip1;
+            qw = (r31 - r13) * sqdip1;
+        }
+        else if (r33 > r11)
+        {
+            float sqdip1 = sqrt(r33 - r11 - r22 + 1.0);
+
+            qz = 0.5 * sqdip1;
+
+            if (sqdip1 != 0)
+                sqdip1 = 0.5 / sqdip1;
+
+            qx = (r31 + r13) * sqdip1;
+            qy = (r23 + r32) * sqdip1;
+            qw = (r12 - r21) * sqdip1;
+        }
+        else
+        {
+            float sqdip1 = sqrt(r11 - r22 - r33 + 1.0);
 
-    Vector4f quat(qx, qy, qz, qw);
+            qx = 0.5 * sqdip1;
 
-    return quat / quat.norm();
+            if (sqdip1 != 0)
+                sqdip1 = 0.5 / sqdip1;
+
+            qy = (r12 + r21) * sqdip1;
+            qz = (r31 + r13) * sqdip1;
+            qw = (r23 - r32) * sqdip1;
+        }
+    }
+
+    return Vector4f(qx, qy, qz, qw);
 }
 
-bool SkyQuaternion::quatnormalize(Vector4f& quat)
+bool quatNormalize(Vector4f& quat)
 {
     float den = sqrt(powf(quat(0), 2) + powf(quat(1), 2) + powf(quat(2), 2) +
                      powf(quat(3), 2));
@@ -173,28 +168,21 @@ bool SkyQuaternion::quatnormalize(Vector4f& quat)
     return true;
 }
 
-Vector4f SkyQuaternion::quatProd(const Vector4f q1, const Vector4f q2)
+Vector4f quatProd(const Vector4f q1, const Vector4f q2)
 {
-    float q1x = q1(0);
-    float q1y = q1(1);
-    float q1z = q1(2);
-    Vector3f qv1(q1x, q1y, q1z);
-    float q1w = q1(3);
-
-    float q2x = q2(0);
-    float q2y = q2(1);
-    float q2z = q2(2);
-    Vector3f qv2(q2x, q2y, q2z);
-    float q2w = q2(3);
+    Vector3f qv1 = q1.head<3>();
+    float qs1    = q1(3);
+    Vector3f qv2 = q2.head<3>();
+    float qs2    = q2(3);
 
     Vector4f quater;
     // cppcheck-suppress constStatement
-    quater << q1w * qv2 + q2w * qv1 - qv1.cross(qv2), q1w * q2w - qv1.dot(qv2);
-    float quaternionNorm = sqrt(quater(0) * quater(0) + quater(1) * quater(1) +
-                                quater(2) * quater(2) + quater(3) * quater(3));
-    quater               = quater / quaternionNorm;
+    quater << qs1 * qv2 + qs2 * qv1 - qv1.cross(qv2), qs1 * qs2 - qv1.dot(qv2);
+    quater.normalize();
 
     return quater;
 }
 
+}  // namespace SkyQuaternion
+
 }  // namespace Boardcore
diff --git a/src/shared/utils/SkyQuaternion/SkyQuaternion.h b/src/shared/utils/SkyQuaternion/SkyQuaternion.h
new file mode 100644
index 0000000000000000000000000000000000000000..fecab90dcc1aa9e9541bc3ac8de59470a354ac18
--- /dev/null
+++ b/src/shared/utils/SkyQuaternion/SkyQuaternion.h
@@ -0,0 +1,81 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Author: Marco Cella
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Functions for managing quaternions.
+ *
+ * Convention used: x, y, z, w (scalar element as last element)
+ */
+namespace SkyQuaternion
+{
+
+/**
+ * @brief Transform a vector of euler angles (ZYX) to quaternion.
+ *
+ * @param euler The vector of euler angles to be transformed [deg]
+ * @return Transformed quaternion [x, y, z, w]
+ */
+Eigen::Vector4f eul2quat(Eigen::Vector3f euler);
+
+/**
+ * @brief Transform a quaternion to a vector of euler angles (ZYX).
+ *
+ * @param quat The quaternion to be transformed [x, y, z, w]
+ * @return Transformed vector of euler angles [deg]
+ */
+Eigen::Vector3f quat2eul(Eigen::Vector4f quat);
+
+/**
+ * @brief Transform a rotation matrix to a quaternion.
+ *
+ * @param rtm The rotation matrix to be transformed (3x3)
+ * @return Transformed quaternion [x, y, z, w]
+ */
+Eigen::Vector4f rotationMatrix2quat(Eigen::Matrix3f rtm);
+
+/**
+ * @brief Normalize a quaternion.
+ *
+ * @param quat The quaternion to be normalized [x, y, z, w]
+ * @return True if the operation succeeded or not
+ */
+bool quatNormalize(Eigen::Vector4f& quat);
+
+/**
+ * @brief Compute the product of two quaternions.
+ *
+ * @param q1 First factor [x, y, z, w]
+ * @param q2 Second factor [x, y, z, w]
+ * @return The resulting quaternions product [x, y, z, w]
+ */
+Eigen::Vector4f quatProd(const Eigen::Vector4f q1, const Eigen::Vector4f q2);
+
+};  // namespace SkyQuaternion
+
+}  // namespace Boardcore
diff --git a/src/shared/math/Stats.cpp b/src/shared/utils/Stats/Stats.cpp
similarity index 100%
rename from src/shared/math/Stats.cpp
rename to src/shared/utils/Stats/Stats.cpp
diff --git a/src/shared/math/Stats.h b/src/shared/utils/Stats/Stats.h
similarity index 100%
rename from src/shared/math/Stats.h
rename to src/shared/utils/Stats/Stats.h
diff --git a/src/shared/utils/testutils/FakeSpiTypedef.h b/src/shared/utils/TestUtils/FakeSpiTypedef.h
similarity index 98%
rename from src/shared/utils/testutils/FakeSpiTypedef.h
rename to src/shared/utils/TestUtils/FakeSpiTypedef.h
index 46b770c44e3b82eb406d54e5718aac248b2b9b61..dae48554ab5c4ac85e0d6e642f7e723a77b48be0 100644
--- a/src/shared/utils/testutils/FakeSpiTypedef.h
+++ b/src/shared/utils/TestUtils/FakeSpiTypedef.h
@@ -22,7 +22,7 @@
 
 #pragma once
 #include <miosix.h>
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
 
 #include <cstdint>
 #include <vector>
diff --git a/src/shared/utils/testutils/MockGpioPin.h b/src/shared/utils/TestUtils/MockGpioPin.h
similarity index 100%
rename from src/shared/utils/testutils/MockGpioPin.h
rename to src/shared/utils/TestUtils/MockGpioPin.h
diff --git a/src/shared/utils/testutils/MockSPIBus.h b/src/shared/utils/TestUtils/MockSPIBus.h
similarity index 98%
rename from src/shared/utils/testutils/MockSPIBus.h
rename to src/shared/utils/TestUtils/MockSPIBus.h
index 9bdc6129e9e2c13f547fe2bada8c1259738d8b71..2fdf6a948ac106fb0fd09338f850c8ef186d2ca4 100644
--- a/src/shared/utils/testutils/MockSPIBus.h
+++ b/src/shared/utils/TestUtils/MockSPIBus.h
@@ -57,7 +57,7 @@ namespace Boardcore
     "SpiBusInterface must be built using MockGpioPin (-DUSE_MOCK_PERIPHERALS)"
 #endif
 
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
 
 using miosix::FastMutex;
 using miosix::Lock;
@@ -69,10 +69,10 @@ public:
     ~MockSPIBus() {}
 
     // Delete copy/move contructors/operators
-    MockSPIBus(const MockSPIBus&) = delete;
+    MockSPIBus(const MockSPIBus&)            = delete;
     MockSPIBus& operator=(const MockSPIBus&) = delete;
 
-    MockSPIBus(MockSPIBus&&) = delete;
+    MockSPIBus(MockSPIBus&&)            = delete;
     MockSPIBus& operator=(MockSPIBus&&) = delete;
 
     /**
diff --git a/src/shared/utils/testutils/TestHelper.cpp b/src/shared/utils/TestUtils/TestHelper.cpp
similarity index 100%
rename from src/shared/utils/testutils/TestHelper.cpp
rename to src/shared/utils/TestUtils/TestHelper.cpp
diff --git a/src/shared/utils/testutils/TestHelper.h b/src/shared/utils/TestUtils/TestHelper.h
similarity index 96%
rename from src/shared/utils/testutils/TestHelper.h
rename to src/shared/utils/TestUtils/TestHelper.h
index 660838fc04fcf103dd581f828e80b99443e6fe23..2e7592f4273969efca7d3ea6a4d38c70b7d02695 100644
--- a/src/shared/utils/testutils/TestHelper.h
+++ b/src/shared/utils/TestUtils/TestHelper.h
@@ -98,7 +98,7 @@ bool testFSMTransition(FSM_type& fsm, const Event& ev,
 template <class FSM_type>
 bool testFSMAsyncTransition(FSM_type& fsm, const Event& ev, uint8_t topic,
                             void (FSM_type::*expectedState)(const Event&),
-                            EventBroker& broker = sEventBroker)
+                            EventBroker& broker = EventBroker::getInstance())
 {
     broker.post(ev, topic);
     // Wait for the event to be handled
@@ -154,7 +154,7 @@ bool testHSMTransition(HSM_type& hsm, const Event& ev,
 template <class HSM_type>
 bool testHSMAsyncTransition(HSM_type& hsm, const Event& ev, uint8_t topic,
                             State (HSM_type::*expectedState)(const Event&),
-                            EventBroker& broker = sEventBroker)
+                            EventBroker& broker = EventBroker::getInstance())
 {
     broker.post(ev, topic);
     // Wait for the event to be handled
@@ -183,7 +183,7 @@ bool testHSMAsyncTransition(HSM_type& hsm, const Event& ev, uint8_t topic,
  */
 bool expectEvent(uint8_t eventId, uint8_t topic, long long when,
                  long long uncertainty = EVENT_TIMING_UNCERTAINTY,
-                 EventBroker& broker   = sEventBroker);
+                 EventBroker& broker   = EventBroker::getInstance());
 
 /**
  * @brief Waits until the specified event is received or a timeout expires
@@ -197,6 +197,6 @@ bool expectEvent(uint8_t eventId, uint8_t topic, long long when,
  *         false    if the timeout has expired
  */
 bool waitForEvent(uint8_t event, uint8_t topic, long long timeout = 0,
-                  EventBroker& broker = sEventBroker);
+                  EventBroker& broker = EventBroker::getInstance());
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/testutils/TestSensor.h b/src/shared/utils/TestUtils/TestSensor.h
similarity index 83%
rename from src/shared/utils/testutils/TestSensor.h
rename to src/shared/utils/TestUtils/TestSensor.h
index 09cc692d12cd7d08446a9567110de3bb68cc2e18..07b19e98b8c415c4f98e4388d1e42b02dec8414b 100644
--- a/src/shared/utils/testutils/TestSensor.h
+++ b/src/shared/utils/TestUtils/TestSensor.h
@@ -29,9 +29,6 @@
 
 #include <cmath>
 
-using miosix::getTick;
-using miosix::TICK_FREQ;
-
 namespace Boardcore
 {
 
@@ -40,11 +37,14 @@ struct TestData : public TimestampData
     float value;
 
     TestData(float v)
-        : TimestampData{static_cast<uint64_t>(getTick())}, value(v)
+        : TimestampData{static_cast<uint64_t>(miosix::getTick())}, value(v)
     {
     }
 
-    TestData() : TimestampData{static_cast<uint64_t>(getTick())}, value(0.0) {}
+    TestData()
+        : TimestampData{static_cast<uint64_t>(miosix::getTick())}, value(0.0)
+    {
+    }
 };
 
 class TestSensor : public Sensor<TestData>
@@ -60,8 +60,9 @@ public:
     TestData sampleImpl() override
     {
         TRACE("[TestSensor] sampleImpl() \n");
-        return TestData(10 * sin(PI * static_cast<float>(getTick()) /
-                                 static_cast<float>(TICK_FREQ)));
+        return TestData(
+            10 * sin(Constants::PI * static_cast<float>(miosix::getTick()) /
+                     static_cast<float>(miosix::TICK_FREQ)));
     }
 };
 
diff --git a/src/shared/utils/testutils/ThroughputCalculator.h b/src/shared/utils/TestUtils/ThroughputCalculator.h
similarity index 100%
rename from src/shared/utils/testutils/ThroughputCalculator.h
rename to src/shared/utils/TestUtils/ThroughputCalculator.h
diff --git a/src/shared/utils/collections/CircularBuffer.h b/src/shared/utils/collections/CircularBuffer.h
index fa6a2e58286996793ee9c7b949b26da61e4cb1db..6182bdcc5b7967af9b90570dada7d62c635438ce 100644
--- a/src/shared/utils/collections/CircularBuffer.h
+++ b/src/shared/utils/collections/CircularBuffer.h
@@ -32,7 +32,7 @@ namespace Boardcore
 {
 
 /**
- * Implementation of an non-synchronized circular buffer
+ * Implementation of an non-synchronized circular buffer.
  */
 template <typename T, unsigned int Size>
 class CircularBuffer
@@ -41,21 +41,25 @@ class CircularBuffer
 
 public:
     CircularBuffer() {}
+
     virtual ~CircularBuffer() {}
 
     /**
-     * Puts a copy of the element in the buffer
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @param elem Element to be added to the queue.
+     * @return The element added.
      */
     virtual T& put(const T& elem)
     {
         buffer[writePtr] = elem;
         T& added         = buffer[writePtr];
 
+        // Advance the read pointer if the two pointers match
         if (!empty && writePtr == readPtr)
-        {
             readPtr = (readPtr + 1) % Size;
-        }
+
+        // Advance the write pointer
         writePtr = (writePtr + 1) % Size;
 
         empty = false;
@@ -64,20 +68,19 @@ public:
     }
 
     /**
-     * Gets an element from the buffer, without removing it
-     * Index starts from the oldest element in the buffer: get(0) returns the
-     * same element as get()
+     * @brief Gets an element from the buffer, without removing it.
      *
-     * @warning Remember to catch the exception!
-     * @throw range_error if index >= count()
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
-     * @param i Index of the elemnt to get, starting from the oldest
-     *
-     * @return the element
+     * @warning Remember to catch the exception!
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    virtual T& get(unsigned int i)
+    virtual T& get(unsigned int i = 0)
     {
-        if (i < CircularBuffer<T, Size>::count())
+        if (i < count())
         {
             int ptr = (readPtr + i) % Size;
             return buffer[ptr];
@@ -89,33 +92,18 @@ public:
     /**
      * @brief Returns the last element added in the buffer.
      *
-     * @throw range_error if buffer is empty
      * @warning Remember to catch the exception!
-     * @return the element
+     * @throw range_error if buffer is empty.
+     * @return The element.
      */
     virtual T& last() { return get(count() - 1); }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @throw range_error if buffer is empty
-     * @warning Remember to catch the exception!
-     * @return the element
-     */
-    virtual T& get()
-    {
-        if (!empty)
-        {
-            return buffer[readPtr];
-        }
-        else
-            throw range_error("CircularBuffer is empty!");
-    }
-
-    /**
-     * Pops the first element in the buffer.
-     * @throw range_error if buffer is empty
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     virtual const T& pop()
     {
@@ -133,8 +121,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     virtual size_t count() const
     {
@@ -157,16 +146,19 @@ public:
         return CircularBuffer<T, Size>::count() == Size;
     }
     /**
-     * Returns the maximum number of elements that can be stored in the buffer
-     * @return buffer size
+     * @brief Returns the maximum number of elements that can be stored in the
+     * buffer.
+     *
+     * @return Buffer size.
      */
     size_t getSize() const { return Size; }
 
 protected:
     T buffer[Size];
 
-    size_t writePtr = 0, readPtr = 0;
-    bool empty = true;
+    size_t writePtr = 0;
+    size_t readPtr  = 0;
+    bool empty      = true;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/collections/IRQCircularBuffer.h b/src/shared/utils/collections/IRQCircularBuffer.h
index a6a02e63664400c7b5efc1fc9af1287613862598..e27d15e2a37652df6e822d29fd8d3ff0cd3af104 100644
--- a/src/shared/utils/collections/IRQCircularBuffer.h
+++ b/src/shared/utils/collections/IRQCircularBuffer.h
@@ -34,7 +34,8 @@ namespace Boardcore
 {
 
 /**
- * Implementation of a synchronized circular buffer
+ * Implementation of a synchronized circular buffer that can be used inside
+ * interrupt service routines.
  */
 template <typename T, unsigned int Size>
 class IRQCircularBuffer : public CircularBuffer<T, Size>
@@ -44,8 +45,7 @@ class IRQCircularBuffer : public CircularBuffer<T, Size>
 
 public:
     /**
-     * Puts a copy of the element in the buffer
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
      */
     T& put(const T& elem) override
     {
@@ -55,37 +55,28 @@ public:
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& get() override
-    {
-        FastInterruptDisableLock d;
-        return Super::get();
-    }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& get(unsigned int i) override
+    T& get(unsigned int i = 0) override
     {
         FastInterruptDisableLock d;
         return Super::get(i);
     }
 
     /**
-     * Pops the first element in the buffer.
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& pop() override
     {
@@ -94,8 +85,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t count() const override
     {
@@ -116,9 +108,9 @@ public:
     }
 
     /**
-     * Puts a copy of the element in the buffer
-     * Only to be called inside an ISR or with interrupts disabled
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     T& IRQput(const T& elem)
     {
@@ -127,8 +119,10 @@ public:
     }
 
     /**
-     * Puts a copy of the element in the buffer
-     * Only to be called inside an ISR or with interrupts disabled
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
      * @param elem element
      * @param hppw Set to true if the woken thread is higher priority than the
      * current one, unchanged otherwise
@@ -137,68 +131,59 @@ public:
     {
         if (waiting && (waiting->IRQgetPriority() >
                         Thread::IRQgetCurrentThread()->IRQgetPriority()))
-        {
             hppw = true;
-        }
 
         IRQwakeWaitingThread();
         return Super::put(elem);
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * Only to be called inside an ISR or with interrupts disabled
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& IRQget() { return Super::get(); }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
-     * @warning Only to be called inside an ISR or with interrupts disabled
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
+     *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& IRQget(unsigned int i) { return Super::get(i); }
+    T& IRQget(unsigned int i = 0) { return Super::get(i); }
 
     /**
-     * Pops the first element in the buffer.
-     * @warning Only to be called inside an ISR or with interrupts disabled
+     * @brief Pops the first element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& IRQpop() { return Super::pop(); }
 
     /**
-     * Counts the elements in the buffer
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t IRQcount() const { return Super::count(); }
 
     /**
-     * @brief Returns true if the buffer is empty
-     *
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return empty or not
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     bool IRQisEmpty() const { return Super::isEmpty(); }
 
     /**
-     * @brief Returns true if the buffer is full
-     *
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return buffer full or not
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     bool IRQisFull() const { return Super::isFull(); }
 
     /**
-     * @brief Waits until the buffer contains at least one element
+     * @brief Waits until the buffer contains at least one element.
      */
     void waitUntilNotEmpty()
     {
@@ -228,4 +213,5 @@ private:
 
     Thread* waiting = nullptr;
 };
-}  // namespace Boardcore
\ No newline at end of file
+
+}  // namespace Boardcore
diff --git a/src/shared/utils/collections/SyncCircularBuffer.h b/src/shared/utils/collections/SyncCircularBuffer.h
index 9e05227df79823f100b0ff915530c451fd0862b8..cab438fbb629c00aa98122ed9b3512deca329e57 100644
--- a/src/shared/utils/collections/SyncCircularBuffer.h
+++ b/src/shared/utils/collections/SyncCircularBuffer.h
@@ -55,37 +55,28 @@ public:
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& get() override
-    {
-        Lock<FastMutex> l(mutex);
-        return Super::get();
-    }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& get(unsigned int i) override
+    T& get(unsigned int i = 0) override
     {
         Lock<FastMutex> l(mutex);
         return Super::get(i);
     }
 
     /**
-     * Pops the first element in the buffer.
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& pop() override
     {
@@ -94,8 +85,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t count() const override
     {
@@ -116,7 +108,7 @@ public:
     }
 
     /**
-     * @brief Waits until the buffer contains at least one element
+     * @brief Waits until the buffer contains at least one element.
      */
     void waitUntilNotEmpty()
     {
diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h
index ff5e7b360a3f612cf73feae05e0283267d96108f..974979585047db698365923d7077444ea6f0d9a5 100644
--- a/src/shared/utils/collections/SyncPacketQueue.h
+++ b/src/shared/utils/collections/SyncPacketQueue.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Alvise de'Faveri Tron
+/* Copyright (c) 2019-2022 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron, Davide Mor, 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
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
 #include <utils/Debug.h>
 
@@ -46,10 +47,9 @@ namespace Boardcore
  * @brief The Packet class is used for packing together messages with variable
  * lengths into a fixed size packet. Useful for telemetry.
  *
- * The buffer can only be appended, read or flushed. The caller can also mark
- * the packet as ready to be sent.
+ * Data can only be appended to the payload. The packet can be marked ready.
  *
- * @tparam len Maximum length for the packet.
+ * @tparam len Packet's payload length.
  */
 template <unsigned int len>
 class Packet
@@ -59,7 +59,7 @@ public:
     /**
      * @brief Reserves a fixed length for the packet.
      */
-    Packet() : msgCounter(0), ts(0), ready(false) { content.reserve(len); };
+    Packet() { content.reserve(len); };
 
     /**
      * @brief Clears the buffer.
@@ -67,24 +67,25 @@ public:
     ~Packet() { content.clear(); };
 
     /**
-     * @brief Try to append a given message to the packet.
+     * @brief Append a given message to the packet.
      *
-     * If it's the first message, also set the timestamp.
+     * If the message can't fit inside the remaining space only the first bytes
+     * are copied.
      *
      * @param msg The message to be appended.
-     * @param msgLen Length of msg.
-     * @return true if the message was appended correctly.
-     * @return false if there isn't enough space for the message.
+     * @param msgLen Length of the message.
+     * @return How many bytes were actually appended.
      */
-    bool tryAppend(const uint8_t* msg, const size_t msgLen);
+    size_t append(const uint8_t* msg, size_t msgLen);
 
     /**
-     * @brief Mark the packet as ready to be sent.
+     * @brief Mark the packet as ready.
      */
     inline void markAsReady() { ready = true; }
 
     /**
-     * @brief Copies the content of the buffer at a given address.
+     * @brief Copies the content of the payload at the given address.
+     *
      * @param buf Where to copy the content.
      * @return How many bytes where copied, i.e. the size of the packet.
      */
@@ -111,12 +112,12 @@ public:
     inline bool isReady() const { return ready; }
 
     /**
-     * @return The timestamp of the first successful call to tryAppend().
+     * @return The timestamp of the first successful call to append().
      */
-    inline uint64_t timestamp() const { return ts; }
+    inline uint64_t getTimestamp() const { return timestamp; }
 
     /**
-     * @return The occupied portion of the buffer (bytes).
+     * @return The occupied portion of the buffer [bytes].
      */
     inline size_t size() const { return content.size(); }
 
@@ -126,7 +127,7 @@ public:
     inline unsigned int maxSize() const { return len; }
 
     /**
-     * @return How many times the tryAppend() function has been called
+     * @return How many times the append() function has been called
      * successfully.
      */
     inline unsigned int getMsgCount() const { return msgCounter; }
@@ -134,7 +135,7 @@ public:
     /**
      * @brief Print information about this object.
      *
-     * @param os For example, std::cout
+     * @param os For example, std::cout.
      */
     void print(std::ostream& os) const;
 
@@ -142,32 +143,29 @@ public:
     std::vector<uint8_t> content;
 
 private:
-    unsigned int msgCounter;
-    uint64_t ts;
-    bool ready;
+    unsigned int msgCounter = 0;
+    uint64_t timestamp      = 0;
+    bool ready              = false;
 };
 
 template <unsigned int len>
-bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen)
+size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
 {
-    if (msgLen == 0 || content.size() + msgLen > len)
-    {
-        return false;
-    }
-    else
+    size_t remaining = len - content.size();
+    msgLen           = std::min(remaining, msgLen);
+
+    if (msgLen != 0)
     {
         // Set the packet's timestamp when the first message is inserted
         if (content.size() == 0)
-        {
-            ts = miosix::getTick();
-        }
+            timestamp = TimestampTimer::getInstance().getTimestamp();
 
         // Append the message to the packet
         content.insert(content.end(), msg, msg + msgLen);
         msgCounter++;
-
-        return true;
     }
+
+    return msgLen;
 }
 
 template <unsigned int len>
@@ -175,7 +173,7 @@ void Packet<len>::clear()
 {
     content.clear();
     msgCounter = 0;
-    ts         = 0;
+    timestamp  = 0;
     ready      = false;
 }
 
@@ -189,194 +187,167 @@ size_t Packet<len>::dump(uint8_t* buf)
 template <unsigned int len>
 void Packet<len>::print(std::ostream& os) const
 {
-    os << "timestamp=" << ts << ", ready=" << ready
+    os << "timestamp=" << timestamp << ", ready=" << ready
        << ", size=" << content.size() << ", msgCounter=" << msgCounter
        << ", content= ";
 
     for (auto const& i : content)
-    {
         os << i;
-    }
     os << '\n';
 }
 
-/******************************************************************************
- * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. The difference
- * is that you pop() Packets but you append() bytes. The bytes will be appended
- * to the first available packet. This class is suitable for synchronization
- * between two threads.
+/**
+ * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets.
  *
- * @tparam pktLen  Maximum length of each packet. (bytes)
- * @tparam pktNum  Total number of packets.
- ******************************************************************************/
+ * The difference is that you pop() Packets but you append() bytes. The bytes
+ * will be appended to the first available packet and the next ones.
+ * This class is suitable for synchronization between two threads.
+ *
+ * @tparam pktLen Maximum length of each packet [bytes].
+ * @tparam pktNum Total number of packets.
+ */
 template <unsigned int pktLen, unsigned int pktNum>
 class SyncPacketQueue
 {
-    using Pkt = Packet<pktLen>;
-
 public:
     /**
-     * @brief Try to append a given message to the last packet. If there isn't
-     * enough space, the packet is marked as ready and the message is appended
-     * to the next packet. If there are no more available packets, the oldest
-     * one is overwritten.
+     * @brief Try to append a given message to the packets queue.
      *
-     * @param msg      the message to be appended
-     * @param msgLen  length of msg
-     * @return true    if the message was appended correctly
-     * @return false   if there isn't enough space for the message
+     * The message is appended to the last packet and if the space isn't enough,
+     * it is divided into successive packets. If there are no more available
+     * packets, the oldest one is overwritten.
+     *
+     * The message isn't added to the queue if there is no space considering all
+     * the queue packets.
+     *
+     * @param msg The message to be appended.
+     * @param msgLen Length of the message [bytes].
+     * @return True if the message was appended.
      */
-    int put(uint8_t* msg, size_t msgLen)
+    bool put(uint8_t* msg, size_t msgLen)
     {
-        int dropped = 0;
+        // Check if the message is empty
+        if (msgLen == 0)
+            return false;
 
-        if (msgLen == 0 || msgLen > pktLen)
-        {
-            return -1;
-        }
+        // Check if the queue can hold the packet
+        if (msgLen > pktLen * pktNum)
+            return false;
 
         {
+            // Lock the mutex on the buffer
             Lock<FastMutex> l(mutex);
+
             // Add an element if there isn't any
             if (buffer.count() == 0)
-            {
-                buffer.put(Pkt{});
-            }
+                buffer.put({});
 
-            Pkt& last = buffer.last();
+            // Write all the packet
+            while (msgLen > 0)
+            {
+                // If the last packet is ready append a new one
+                if (buffer.last().isReady())
+                    buffer.put({});
 
-            bool added = false;
+                // Append what data is possible to the last packet
+                size_t appendedLength = buffer.last().append(msg, msgLen);
 
-            // Add to the current packet only if it isn't already ready
-            if (!last.isReady())
-            {
-                added = last.tryAppend(msg, msgLen);
-            }
+                // If the packet is full mark it as ready
+                if (buffer.last().isFull())
+                    buffer.last().markAsReady();
 
-            // If already ready or cannot fit the new data, add to a new packet
-            if (!added)
-            {
-                // Mark the packet as ready (in the case it wasn't already)
-                last.markAsReady();
-
-                if (buffer.isFull())
-                {
-                    // We have dropped a packet
-                    ++dropped;
-                }
-                // Add a new packet and fill that instead
-                Pkt& newpkt = buffer.put(Pkt{});
-
-                if (!newpkt.tryAppend(msg, msgLen))
-                {
-                    TRACE("Packet is too big!\n");
-                    return -1;
-                }
+                // Go forward in the data
+                msgLen -= appendedLength;
+                msg += appendedLength;
             }
+        }
 
-            // Mark as ready if the packet is full
-            if (buffer.last().isFull())
-            {
-                buffer.last().markAsReady();
-            }
+        // Wake all waiting threads
+        condVerNotEmpty.broadcast();
 
-            cvNotempty.broadcast();
-            return dropped;
-        }
+        return true;
     }
 
     /**
-     * @return a copy of the oldest packet, without removing it from the queue.
+     * @return The oldest packet, without removing it from the queue.
      */
-    const Pkt& get()
+    const Packet<pktLen>& get()
     {
         Lock<FastMutex> l(mutex);
         return buffer.get();
     }
 
     /**
-     * @return the oldest packet, removing it from the queue.
+     * @return The oldest packet, removing it from the queue.
      */
-    const Pkt& pop()
+    const Packet<pktLen>& pop()
     {
         Lock<FastMutex> l(mutex);
         return buffer.pop();
     }
 
     /**
-     * @return true if all the packets have been marked as ready.
+     * @return True if all the packets have been marked as ready.
      */
     bool isFull()
     {
         Lock<FastMutex> l(mutex);
 
         if (buffer.count() > 0)
-        {
             return buffer.isFull() && buffer.last().isReady();
-        }
         else
-        {
             return false;
-        }
     }
 
     /**
-     * @return true if all the packets are completely empty.
+     * @return True if all the packets are completely empty.
      */
     bool isEmpty()
     {
         Lock<FastMutex> l(mutex);
-
         return buffer.isEmpty();
     }
 
     /**
      * @brief Blocks the calling thread until the queue is not empty.
+     *
      * Returns immediately if already not empty.
      */
     void waitUntilNotEmpty()
     {
         Lock<FastMutex> l(mutex);
         if (buffer.isEmpty())
-        {
-            cvNotempty.wait(mutex);
-        }
+            condVerNotEmpty.wait(mutex);
     }
 
     /**
-     * @return the number of packets that are ready to be sent.
+     * @return The number of packets that are ready to be sent.
      */
     size_t countReady()
     {
         Lock<FastMutex> l(mutex);
 
         if (!buffer.isEmpty())
-        {
             return buffer.last().isReady() ? buffer.count()
                                            : buffer.count() - 1;
-        }
         else
-        {
             return 0;
-        }
     }
 
     /**
-     * @return the number of packets in use, that are either fully or partially
+     * @return The number of packets in use, that are either fully or partially
      * filled.
      */
     size_t countNotEmpty()
     {
         Lock<FastMutex> l(mutex);
-
         return buffer.count();
     }
 
 private:
     FastMutex mutex;
-    ConditionVariable cvNotempty;
-
-    CircularBuffer<Pkt, pktNum> buffer;
+    ConditionVariable condVerNotEmpty;
+    CircularBuffer<Packet<pktLen>, pktNum> buffer;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/collections/SyncQueue.h b/src/shared/utils/collections/SyncQueue.h
index c1b4ccc9a38b4bc2e6ae0d34fd375f5dd0b1eaf2..5f340bba650b6e8f5238e4cc8b8be02ef7c070ef 100644
--- a/src/shared/utils/collections/SyncQueue.h
+++ b/src/shared/utils/collections/SyncQueue.h
@@ -39,7 +39,7 @@ public:
     int len();
 
 private:
-    SynchronizedQueue(const SynchronizedQueue&) = delete;
+    SynchronizedQueue(const SynchronizedQueue&)            = delete;
     SynchronizedQueue& operator=(const SynchronizedQueue&) = delete;
     std::list<T> queue;
     miosix::Mutex mMutex;
diff --git a/src/shared/utils/collections/contiguous_queue.h b/src/shared/utils/collections/contiguous_queue.h
index 039482f1516f9b29505973222a6275f6a6b46044..bc63a664d11bb5240eda743fe7750f340ea1cb18 100644
--- a/src/shared/utils/collections/contiguous_queue.h
+++ b/src/shared/utils/collections/contiguous_queue.h
@@ -128,7 +128,7 @@ public:
     }
 
 private:
-    ContiguousQueue(const ContiguousQueue&) = delete;
+    ContiguousQueue(const ContiguousQueue&)            = delete;
     ContiguousQueue& operator=(const ContiguousQueue&) = delete;
 
     T elements[N]     = {0};
diff --git a/src/shared/utils/gui/NavController.h b/src/shared/utils/gui/NavController.h
index 528692b3342b3f807465d64099345eb356fea937..617af98d72b4cf4914a08b2a9098e2188ae830cd 100644
--- a/src/shared/utils/gui/NavController.h
+++ b/src/shared/utils/gui/NavController.h
@@ -23,7 +23,7 @@
 #pragma once
 
 #include <diagnostic/PrintLogger.h>
-#include <utils/ButtonHandler.h>
+#include <utils/ButtonHandler/ButtonHandler.h>
 #include <utils/Debug.h>
 
 #include <vector>
@@ -66,43 +66,37 @@ public:
         }
     }
 
-    void onButtonPress(ButtonPress press)
+    void onButtonEvent(ButtonEvent press)
     {
         switch (press)
         {
-            case ButtonPress::SHORT:
-                if (vecSelectable.size() > 0)
-                {
-                    selectNext();
-                }
-                break;
-            case ButtonPress::DOWN:
+            case ButtonEvent::PRESSED:
                 if (selectedIndex < vecSelectable.size())
-                {
                     vecSelectable.at(selectedIndex)
                         ->performInteraction(Interaction::BTN_DOWN);
-                }
+                if (vecSelectable.size() > 0)
+                    selectNext();
                 break;
-            case ButtonPress::UP:
+            case ButtonEvent::SHORT_PRESS:
                 if (selectedIndex < vecSelectable.size())
-                {
                     vecSelectable.at(selectedIndex)
                         ->performInteraction(Interaction::BTN_UP);
-                }
+                if (vecSelectable.size() > 0)
+                    selectNext();
                 break;
-            case ButtonPress::LONG:
+            case ButtonEvent::LONG_PRESS:
                 if (selectedIndex < vecSelectable.size())
-                {
                     vecSelectable.at(selectedIndex)
                         ->performInteraction(Interaction::CLICK);
-                }
+                if (vecSelectable.size() > 0)
+                    selectNext();
                 break;
-            case ButtonPress::VERY_LONG:
+            case ButtonEvent::VERY_LONG_PRESS:
                 if (selectedIndex < vecSelectable.size())
-                {
                     vecSelectable.at(selectedIndex)
                         ->performInteraction(Interaction::LONG_CLICK);
-                }
+                if (vecSelectable.size() > 0)
+                    selectNext();
                 break;
             default:
                 break;
diff --git a/src/shared/utils/gui/ScreenManager.h b/src/shared/utils/gui/ScreenManager.h
index 4553fed13ea7126b924a1d4c9d9df008c3957092..ce731f437f638bdf904b44e0f5d1fb89ff11a69b 100644
--- a/src/shared/utils/gui/ScreenManager.h
+++ b/src/shared/utils/gui/ScreenManager.h
@@ -65,7 +65,7 @@ public:
         }
     }
 
-    void onButtonPress(ButtonPress press) { controller.onButtonPress(press); }
+    void onButtonEvent(ButtonEvent press) { controller.onButtonEvent(press); }
 
     mxgui::DrawingContext& getDrawingContext() { return dc; }
 
@@ -86,7 +86,7 @@ protected:
 
             drawViewTree(screens[activeScreen], dc);
 
-            Thread::sleepUntil(start + refreshInterval);
+            miosix::Thread::sleepUntil(start + refreshInterval);
         }
     }
 
diff --git a/src/tests/drivers/test-servo.cpp b/src/tests/actuators/test-buzzer.cpp
similarity index 55%
rename from src/tests/drivers/test-servo.cpp
rename to src/tests/actuators/test-buzzer.cpp
index 10eaf9c81ba9e43dbdcfdc7c9db94e9b12ec7801..fc6f6fc6bafecae80a801f91b1115223891c31f7 100644
--- a/src/tests/drivers/test-servo.cpp
+++ b/src/tests/actuators/test-buzzer.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta, Alberto Nidasio
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -20,47 +20,33 @@
  * THE SOFTWARE.
  */
 
-#include <drivers/servo/Servo.h>
+#include <actuators/Buzzer.h>
 #include <miosix.h>
-#include <utils/ClockUtils.h>
 
-using namespace Boardcore;
 using namespace miosix;
-
-using ps1 = Gpio<GPIOB_BASE, 4>;
-using ps2 = Gpio<GPIOA_BASE, 7>;
-using ps3 = Gpio<GPIOB_BASE, 8>;
+using namespace Boardcore;
 
 int main()
 {
-    ps1::mode(Mode::ALTERNATE);
-    ps2::mode(Mode::ALTERNATE);
-    ps3::mode(Mode::ALTERNATE);
-
-    ps1::alternateFunction(2);
-    ps2::alternateFunction(2);
-    ps3::alternateFunction(3);
-
-    Servo s1(TIM3, TimerUtils::Channel::CHANNEL_1);
-    Servo s2(TIM3, TimerUtils::Channel::CHANNEL_2);
-    Servo s3(TIM10, TimerUtils::Channel::CHANNEL_1);
+    Buzzer buzzer(TIM4, TimerUtils::Channel::CHANNEL_2);
 
-    s1.enable();
-    s2.enable();
-    s3.enable();
+    printf("Now beeping for 2 seconds\n");
+    buzzer.oneTimeToggle(2 * 1000);
+    Thread::sleep(3 * 1000);
 
-    float pos[] = {0, 0.5, 1.0};
+    printf("Now continuosly toggle the buzzer every 500ms for 5 seconds");
+    buzzer.continuoslyToggle(500, 500);
+    Thread::sleep(6 * 1000);
 
-    s1.setPosition(pos[0]);
-    s2.setPosition(pos[1]);
-    s3.setPosition(pos[2]);
+    printf("Now continuosly toggle the buzzer every 200ms for 5 seconds");
+    buzzer.continuoslyToggle(200, 200);
+    Thread::sleep(6 * 1000);
 
-    for (int i = 0;; i++)
-    {
-        s1.setPosition(pos[i % 3]);
-        s2.setPosition(pos[(i + 1) % 3]);
-        s3.setPosition(pos[(i + 2) % 3]);
+    printf("Now manually turning on the buzzer for 1 second\n");
+    buzzer.on();
+    Thread::sleep(1000);
+    buzzer.off();
 
-        Thread::sleep(2000);
-    }
+    while (true)
+        Thread::sleep(1000);
 }
diff --git a/src/tests/drivers/test-hbridge.cpp b/src/tests/actuators/test-hbridge.cpp
similarity index 98%
rename from src/tests/drivers/test-hbridge.cpp
rename to src/tests/actuators/test-hbridge.cpp
index 0f2eb7c18ce9de005af15b1a216743ce02529f58..a2aa0438aa91015c899273a37c38ff87458def55 100644
--- a/src/tests/drivers/test-hbridge.cpp
+++ b/src/tests/actuators/test-hbridge.cpp
@@ -20,7 +20,7 @@
  * THE SOFTWARE.
  */
 
-#include <drivers/hbridge/HBridge.h>
+#include <actuators/HBridge/HBridge.h>
 #include <miosix.h>
 #include <utils/Debug.h>
 
diff --git a/src/tests/actuators/test-servo.cpp b/src/tests/actuators/test-servo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3606640a5468e02a6e8b2dae060b440640a70bbe
--- /dev/null
+++ b/src/tests/actuators/test-servo.cpp
@@ -0,0 +1,101 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <actuators/Servo/Servo.h>
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/ClockUtils.h>
+
+#include <iostream>
+
+/**
+ * The test uses 4 gpio pins:
+ * - PB4: TIM3-CH1
+ * - PA7: TIM3-CH2
+ * - PC8: TIM3-CH3
+ * - PB7: TIM4-CH2
+ */
+
+using namespace Boardcore;
+using namespace miosix;
+
+GpioPin pin1(GPIOB_BASE, 4);
+GpioPin pin2(GPIOA_BASE, 7);
+GpioPin pin3(GPIOC_BASE, 8);
+GpioPin pin4(GPIOB_BASE, 7);
+
+Servo s1(TIM3, TimerUtils::Channel::CHANNEL_1);
+Servo s2(TIM3, TimerUtils::Channel::CHANNEL_2);
+Servo s3(TIM3, TimerUtils::Channel::CHANNEL_3);
+Servo s4(TIM4, TimerUtils::Channel::CHANNEL_2);
+
+// Position to cycle through for the servo 1, 2 and 3
+float positions[] = {0, 0.5, 1.0};
+int lastPosition  = 0;
+
+void moveServo()
+{
+    s1.setPosition(positions[lastPosition % 3]);
+    s2.setPosition(positions[(lastPosition + 1) % 3]);
+    s3.setPosition(positions[(lastPosition + 2) % 3]);
+
+    lastPosition++;
+}
+
+int main()
+{
+    pin1.mode(Mode::ALTERNATE);
+    pin1.alternateFunction(2);
+    pin2.mode(Mode::ALTERNATE);
+    pin2.alternateFunction(2);
+    pin3.mode(Mode::ALTERNATE);
+    pin3.alternateFunction(2);
+    pin4.mode(Mode::ALTERNATE);
+    pin4.alternateFunction(2);
+
+    // Enable the timers
+    s1.enable();
+    s2.enable();
+    s3.enable();
+    s4.enable();
+
+    // Start a periodic task to move the first three servos
+    TaskScheduler scheduler;
+    scheduler.addTask(&moveServo, 2 * 1000, 1);
+    scheduler.start();
+
+    // Control the fourth servo manually
+    float percentage;
+
+    printf("Please enter a percentage for the servo ");
+    printf("or anything else to exit: ");
+
+    while (std::cin >> percentage)
+    {
+        s4.setPosition(percentage / 100);
+
+        printf("Position set to: %.2f%%\n", percentage);
+
+        printf("Please enter a percentage for the servo ");
+        printf("or anything else to exit: ");
+    }
+}
diff --git a/src/tests/drivers/stepper/test-stepper.cpp b/src/tests/actuators/test-stepper.cpp
similarity index 99%
rename from src/tests/drivers/stepper/test-stepper.cpp
rename to src/tests/actuators/test-stepper.cpp
index 3b27f5f0f565a98e6c41882148f92e31f2ef2ae2..707f227892df7545e5aec8b6a9ef174322ed8e46 100644
--- a/src/tests/drivers/stepper/test-stepper.cpp
+++ b/src/tests/actuators/test-stepper.cpp
@@ -20,7 +20,7 @@
  * THE SOFTWARE.
  */
 
-#include <drivers/stepper/Stepper.h>
+#include <actuators/Stepper.h>
 
 using namespace miosix;
 using namespace Boardcore;
@@ -125,4 +125,4 @@ int main()
         doRoutine(stepper);
         printf("The stepper is now disabled\n");
     }
-}
\ No newline at end of file
+}
diff --git a/src/tests/kalman/test-kalman-data.h b/src/tests/algorithms/Kalman/test-kalman-data.h
similarity index 100%
rename from src/tests/kalman/test-kalman-data.h
rename to src/tests/algorithms/Kalman/test-kalman-data.h
diff --git a/src/tests/kalman/test-kalman-eigen-benchmark.cpp b/src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp
similarity index 95%
rename from src/tests/kalman/test-kalman-eigen-benchmark.cpp
rename to src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp
index 33d468abe355c658e0903c5ec7c5ca845241332c..7b47c1b28b1e716ed826ce55c6ae9787b6e9e106 100644
--- a/src/tests/kalman/test-kalman-eigen-benchmark.cpp
+++ b/src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp
@@ -25,12 +25,12 @@
 
 #define EIGEN_RUNTIME_NO_MALLOC
 
-#include <algorithms/kalman/KalmanEigen.h>
+#include <algorithms/Kalman/Kalman.h>
 #include <drivers/timer/GeneralPurposeTimer.h>
 #include <drivers/timer/TimerUtils.h>
 #include <kernel/kernel.h>
-#include <math/SkyQuaternion.h>
 #include <util/util.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
 
 #include <iostream>
 
@@ -71,7 +71,7 @@ int main()
 
     Matrix<float, p, 1> y(p);  // vector with p elements (only one in this case)
 
-    KalmanEigen<float, n, p>::KalmanConfig config;
+    Kalman<float, n, p>::KalmanConfig config;
     config.F = F;
     config.H = H;
     config.Q = Q;
@@ -79,7 +79,7 @@ int main()
     config.P = P;
     config.x = x0;
 
-    KalmanEigen<float, n, p> filter(config);
+    Kalman<float, n, p> filter(config);
 
     float lastTime = 0.0;  // Variable to save the time of the last sample
 
diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f8af2b692b965d8873f721c62d57308931475a8
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
@@ -0,0 +1,141 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void imuInit();
+void imuStep();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu = nullptr;
+
+int main()
+{
+    imuInit();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(imuStep, 20);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+    config.NED_MAG        = nedMag;
+
+    return config;
+}
+
+void setInitialOrientation()
+{
+    Eigen::Matrix<float, 13, 1> x;
+
+    // Set quaternions
+    Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+    x(6)              = q(0);
+    x(7)              = q(1);
+    x(8)              = q(2);
+    x(9)              = q(3);
+
+    nas->setX(x);
+}
+
+void imuInit()
+{
+    imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    imu->init();
+}
+
+void imuStep()
+{
+    imu->sample();
+    auto data = imu->getLastSample();
+    Vector3f acceleration(data.accelerationX, data.accelerationY,
+                          data.accelerationZ);
+    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                             data.angularVelocityZ);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Calibration
+    {
+        Vector3f bias(0.0218462708018154, 0.0281755574886535,
+                      0.0264119470499244);
+        angularVelocity -= bias;
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+    }
+
+    acceleration.normalize();
+    magneticField.normalize();
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+
+    // Correct step
+    nas->correctMag(magneticField);
+    nas->correctAcc(acceleration);
+
+    auto nasState = nas->getState();
+    printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+           nasState.qz);
+}
diff --git a/src/tests/algorithms/NAS/test-attitude-stack.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..beacab318c26c6005805ca9f7bc24aa7e1147e6b
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-attitude-stack.cpp
@@ -0,0 +1,167 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/SensorManager.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void imuInit();
+void imuStep();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+BMX160* imu = nullptr;
+
+int main()
+{
+    imuInit();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(imuStep, 20);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+    config.NED_MAG        = nedMag;
+
+    return config;
+}
+
+void setInitialOrientation()
+{
+    Eigen::Matrix<float, 13, 1> x;
+
+    // Set quaternions
+    Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+    x(6)              = q(0);
+    x(7)              = q(1);
+    x(8)              = q(2);
+    x(9)              = q(3);
+
+    nas->setX(x);
+}
+
+void imuInit()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmx_config;
+    bmx_config.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark = 80;
+    bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    bmx_config.temperatureDivider = 1;
+
+    bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+
+    bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+
+    bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    imu = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                     spiConfig);
+    imu->init();
+}
+
+void imuStep()
+{
+    imu->sample();
+    auto data = imu->getLastSample();
+    Vector3f acceleration(data.accelerationX, data.accelerationY,
+                          data.accelerationZ);
+    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                             data.angularVelocityZ);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Calibration
+    {
+        Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+        angularVelocity = angularVelocity - offset;
+        angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+        Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+        Matrix3f A{{0.688760050772712, 0, 0},
+                   {0, 0.637715211784480, 0},
+                   {0, 0, 2.27669720320908}};
+        magneticField = (magneticField - b).transpose() * A;
+    }
+
+    acceleration.normalize();
+    magneticField.normalize();
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+
+    // Correct step
+    nas->correctMag(magneticField);
+    nas->correctAcc(acceleration);
+
+    // std::cout << acceleration.transpose() << angularVelocity.transpose()
+    //           << magneticField.transpose() << std::endl;
+
+    auto nasState = nas->getState();
+    printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+           nasState.qz);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57fd167c839ec3bdef2d24c69417ddb7f118aab1
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -0,0 +1,183 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void init();
+void step();
+void print();
+
+Vector3f nedMag   = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu  = nullptr;
+UbloxGPS* gps = nullptr;
+
+int main()
+{
+    init();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+    scheduler.addTask(print, 250);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+    config.NED_MAG        = nedMag;
+
+    return config;
+}
+
+void setInitialOrientation()
+{
+    Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+    // Set quaternions
+    Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+    x(6)       = q(0);
+    x(7)       = q(1);
+    x(8)       = q(2);
+    x(9)       = q(3);
+
+    nas->setX(x);
+}
+
+void init()
+{
+    imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    imu->init();
+
+    gps = new UbloxGPS(38400, 10, 2, "gps", 38400);
+    gps->init();
+    gps->start();
+
+    Logger::getInstance().start();
+}
+
+void step()
+{
+    imu->sample();
+    gps->sample();
+    auto imuData = imu->getLastSample();
+    auto gpsData = gps->getLastSample();
+
+    Vector3f acceleration(imuData.accelerationX, imuData.accelerationY,
+                          imuData.accelerationZ);
+    Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY,
+                             imuData.angularVelocityZ);
+    Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY,
+                           imuData.magneticFieldZ);
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+    Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth);
+    Vector4f gpsCorrection;
+    // cppcheck-suppress constStatement
+    gpsCorrection << gpsPos, gpsVel;
+
+    // Calibration
+    {
+        Vector3f biasAcc(-0.1255, 0.2053, -0.2073);
+        acceleration -= biasAcc;
+        Vector3f bias(-0.0291, 0.0149, 0.0202);
+        angularVelocity -= bias;
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+    }
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+    if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 &&
+        gpsPos[1] > -1e3)
+        nas->predictAcc(acceleration);
+
+    // Correct step
+    magneticField.normalize();
+    nas->correctMag(magneticField);
+    acceleration.normalize();
+    nas->correctAcc(acceleration);
+    if (gpsData.fix)
+        nas->correctGPS(gpsCorrection);
+    nas->correctBaro(100000, 110000, 20 + 273.5);
+
+    auto nasState = nas->getState();
+    Logger::getInstance().log(imuData);
+    Logger::getInstance().log(gpsData);
+    Logger::getInstance().log(nasState);
+}
+
+void print()
+{
+    auto gpsData  = gps->getLastSample();
+    auto nasState = nas->getState();
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+    printf("%d, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0], gpsPos[1],
+           nasState.n, nasState.e, nasState.vn, nasState.ve);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-stack.cpp b/src/tests/algorithms/NAS/test-nas-stack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e72a1c9830b63682cffff7968cbc27106b85a61f
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-stack.cpp
@@ -0,0 +1,216 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MS5803/MS5803.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void init();
+void step();
+void print();
+
+Vector3f nedMag   = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+BMX160* imu   = nullptr;
+UbloxGPS* gps = nullptr;
+MS5803* baro  = nullptr;
+
+int main()
+{
+    init();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+    // scheduler.addTask(print, 250);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+    config.NED_MAG        = nedMag;
+
+    return config;
+}
+
+void setInitialOrientation()
+{
+    Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+    // Set quaternions
+    Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+    x(6)       = q(0);
+    x(7)       = q(1);
+    x(8)       = q(2);
+    x(9)       = q(3);
+
+    nas->setX(x);
+}
+
+void init()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmx_config;
+    bmx_config.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark = 80;
+    bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    bmx_config.temperatureDivider = 1;
+
+    bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+
+    bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+
+    bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    imu =
+        new BMX160(spi1, sensors::bmx160::cs::getPin(), bmx_config, spiConfig);
+    imu->init();
+
+    gps = new UbloxGPS(921600, 10, 2, "gps", 38400);
+    gps->init();
+    gps->start();
+
+    baro = new MS5803(spi1, sensors::ms5803::cs::getPin());
+    baro->init();
+
+    Logger::getInstance().start();
+}
+
+void step()
+{
+    imu->sample();
+    gps->sample();
+    baro->sample();
+    auto imuData  = imu->getLastSample();
+    auto gpsData  = gps->getLastSample();
+    auto baroData = baro->getLastSample();
+
+    Vector3f acceleration(imuData.accelerationX, imuData.accelerationY,
+                          imuData.accelerationZ);
+    Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY,
+                             imuData.angularVelocityZ);
+    Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY,
+                           imuData.magneticFieldZ);
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+    Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityEast);
+    Vector4f gpsCorrection;
+    // cppcheck-suppress constStatement
+    gpsCorrection << gpsPos, gpsVel;
+
+    // Calibration
+    {
+        Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+        angularVelocity = angularVelocity - offset;
+        angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+        Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+        Matrix3f A{{0.688760050772712, 0, 0},
+                   {0, 0.637715211784480, 0},
+                   {0, 0, 2.27669720320908}};
+        magneticField = (magneticField - b).transpose() * A;
+    }
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+    nas->predictAcc(acceleration);
+
+    // Correct step
+    magneticField.normalize();
+    nas->correctMag(magneticField);
+    acceleration.normalize();
+    nas->correctAcc(acceleration);
+    if (gpsData.fix)
+        nas->correctGPS(gpsCorrection);
+    nas->correctBaro(100000, Constants::MSL_PRESSURE,
+                     Constants::MSL_TEMPERATURE);
+
+    auto nasState = nas->getState();
+
+    Logger::getInstance().log(imuData);
+    Logger::getInstance().log(gpsData);
+    Logger::getInstance().log(baroData);
+    Logger::getInstance().log(nasState);
+}
+
+void print()
+{
+    auto gpsData  = gps->getLastSample();
+    auto baroData = baro->getLastSample();
+    auto nasState = nas->getState();
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+    printf("%d, %f, %f, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0],
+           gpsPos[1], nasState.n, nasState.e, nasState.d, nasState.vn,
+           nasState.ve, baroData.pressure);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59baeb1d1e37a36035e2fe6208ec22617ea8ad2d
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
@@ -0,0 +1,213 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/SensorManager.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void bmxInit();
+void bmxCallback();
+
+constexpr uint64_t CALIBRATION_TIMEOUT = 5 * 1e6;
+
+Vector3f nedMag = Vector3f(0.47472049, 0.02757190, 0.87970463);
+
+StateInitializer* stateInitializer;
+NAS* nas;
+
+SPIBus spi1(SPI1);
+BMX160* bmx160 = nullptr;
+
+Boardcore::SensorManager::SensorMap_t sensorsMap;
+Boardcore::SensorManager* sensorManager = nullptr;
+
+int main()
+{
+    bmxInit();
+
+    sensorManager    = new SensorManager(sensorsMap);
+    stateInitializer = new StateInitializer();
+    nas              = new NAS(getEKConfig());
+
+    // Logger::getInstance().start();
+    TimestampTimer::getInstance().resetTimestamp();
+    sensorManager->start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+
+    // Normalized magnetic field in Milan
+    config.NED_MAG = nedMag;
+
+    return config;
+}
+
+void bmxInit()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmx_config;
+    bmx_config.fifoMode              = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark         = 80;
+    bmx_config.fifoInterrupt         = BMX160Config::FifoInterruptPin::PIN_INT1;
+    bmx_config.temperatureDivider    = 1;
+    bmx_config.accelerometerRange    = BMX160Config::AccelerometerRange::G_16;
+    bmx_config.gyroscopeRange        = BMX160Config::GyroscopeRange::DEG_1000;
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeUnit         = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                        spiConfig);
+
+    SensorInfo info("BMX160", 20, &bmxCallback);
+
+    sensorsMap.emplace(std::make_pair(bmx160, info));
+}
+
+void bmxCallback()
+{
+    static int meanCount    = 0;
+    static bool calibrating = true;
+    static Vector3f accMean = Vector3f::Zero();
+    static Vector3f magMean = Vector3f::Zero();
+
+    auto data = bmx160->getLastSample();
+    Vector3f acceleration(data.accelerationX, data.accelerationY,
+                          data.accelerationZ);
+    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                             data.angularVelocityZ);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Apply correction
+    Vector3f acc_b(0.3763 + 0.094, -0.1445 - 0.0229, -0.1010 + 0.0150);
+    acceleration -= acc_b;
+    Vector3f gyro_b{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+    angularVelocity = angularVelocity - gyro_b;
+    angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+    Vector3f mag_b{21.0730033648425, -24.3997259703105, -2.32621524742862};
+    Matrix3f A{{0.659926504672263, 0, 0},
+               {0, 0.662442130094073, 0},
+               {0, 0, 2.28747567094359}};
+    magneticField = (magneticField - mag_b).transpose() * A;
+
+    if (calibrating)
+    {
+        if (TimestampTimer::getInstance().getTimestamp() < CALIBRATION_TIMEOUT)
+        {
+            accMean = (accMean * meanCount + acceleration) / (meanCount + 1);
+            magMean = (magMean * meanCount + acceleration) / (meanCount + 1);
+            meanCount++;
+        }
+        else
+        {
+            // Now the calibration has ended, compute and log the nas state
+            calibrating = false;
+
+            // Compute the initial nas state
+            stateInitializer->triad(accMean, magMean, nedMag);
+            nas->setX(stateInitializer->getInitX());
+
+            // Save the state and the IMU data
+            // Logger::getInstance().log(nas->getState());
+            data.accelerationX  = accMean[0];
+            data.accelerationY  = accMean[1];
+            data.accelerationZ  = accMean[2];
+            data.magneticFieldX = magMean[0];
+            data.magneticFieldY = magMean[1];
+            data.magneticFieldZ = magMean[2];
+            // Logger::getInstance().log(data);
+
+            // Restart the logger to change log filename
+            // Logger::getInstance().stop();
+            // Logger::getInstance().start();
+        }
+    }
+    else
+    {
+        // Predict step
+        {
+            // nas->predictAcc(acceleration);
+            nas->predictGyro(angularVelocity);
+
+            data.angularVelocityX = angularVelocity[0];
+            data.angularVelocityY = angularVelocity[1];
+            data.angularVelocityZ = angularVelocity[2];
+        }
+
+        // Correct step
+        {
+            magneticField.normalize();
+            nas->correctMag(magneticField);
+
+            data.magneticFieldX = magneticField[0];
+            data.magneticFieldY = magneticField[1];
+            data.magneticFieldZ = magneticField[2];
+        }
+
+        auto nasState = nas->getState();
+
+        nasState.timestamp = TimestampTimer::getInstance().getTimestamp();
+        data.accelerationTimestamp    = nasState.timestamp;
+        data.magneticFieldTimestamp   = nasState.timestamp;
+        data.angularVelocityTimestamp = nasState.timestamp;
+
+        // Logger::getInstance().log(nasState);
+        // Logger::getInstance().log(data);
+        printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+               nasState.qz);
+    }
+}
diff --git a/src/tests/algorithms/NAS/test-tmp.cpp b/src/tests/algorithms/NAS/test-tmp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39fcdbd511705c39361f2a926ef92c15ea662ff9
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-tmp.cpp
@@ -0,0 +1,89 @@
+/* Copyright (c) 2021 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 <drivers/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+
+using namespace Boardcore;
+
+GpioPin mpuCs = GpioPin(GPIOG_BASE, 3);
+SPIBus spi1(SPI1);
+
+MPU9250* mpu = nullptr;
+BMX160* bmx  = nullptr;
+
+void initBoard()
+{
+    // Chip select pin as output starting high
+    mpuCs.mode(miosix::Mode::OUTPUT);
+    mpuCs.high();
+
+    mpu = new MPU9250(spi1, mpuCs);
+
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmxConfig;
+    bmxConfig.fifoMode              = BMX160Config::FifoMode::HEADER;
+    bmxConfig.fifoWatermark         = 80;
+    bmxConfig.fifoInterrupt         = BMX160Config::FifoInterruptPin::PIN_INT1;
+    bmxConfig.temperatureDivider    = 1;
+    bmxConfig.accelerometerRange    = BMX160Config::AccelerometerRange::G_16;
+    bmxConfig.gyroscopeRange        = BMX160Config::GyroscopeRange::DEG_1000;
+    bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeUnit         = BMX160Config::GyroscopeMeasureUnit::RAD;
+    bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmxConfig,
+                     spiConfig);
+
+    mpu->init();
+    bmx->init();
+}
+
+int main()
+{
+    initBoard();
+
+    auto lastTick = getTick();
+    while (true)
+    {
+        mpu->sample();
+        bmx->sample();
+        auto mpuData = mpu->getLastSample();
+        auto bmxData = bmx->getLastSample();
+
+        printf("%f,%f,%f,", mpuData.magneticFieldX, mpuData.magneticFieldY,
+               mpuData.magneticFieldZ);
+        printf("%f,%f,%f\n", bmxData.magneticFieldX, bmxData.magneticFieldY,
+               bmxData.magneticFieldZ);
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
+}
diff --git a/src/tests/algorithms/NAS/test-triad-parafoil.cpp b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed93635ffcb04004de8ce7810051b48a21bb08bb
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
@@ -0,0 +1,79 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/SensorDataExtra.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+void mpuInit();
+void bmxCallback();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+SPIBus spi1(SPI1);
+MPU9250* mpu = nullptr;
+
+int main()
+{
+    mpu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    mpu->init();
+
+    auto lastTick = getTick();
+    while (true)
+    {
+        mpu->sample();
+        auto data = mpu->getLastSample();
+
+        Vector3f acceleration(data.accelerationX, data.accelerationY,
+                              data.accelerationZ);
+
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                               data.magneticFieldZ);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+
+        acceleration.normalize();
+        magneticField.normalize();
+
+        StateInitializer state;
+        state.triad(acceleration, magneticField, nedMag);
+
+        auto kalmanState = state.getInitX();
+        if (!std::isnan(kalmanState(9)))
+            printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+                   kalmanState(7), kalmanState(8));
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
+}
diff --git a/src/tests/algorithms/NAS/test-triad.cpp b/src/tests/algorithms/NAS/test-triad.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5104a1e0f2cb838a1b14f7b58ec072fbabcdcab
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-triad.cpp
@@ -0,0 +1,110 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/SensorDataExtra.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+void imuInit();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+SPIBus spi1(SPI1);
+BMX160* bmx = nullptr;
+
+int main()
+{
+    imuInit();
+    bmx->init();
+
+    auto lastTick = getTick();
+    while (true)
+    {
+        bmx->sample();
+        auto data = bmx->getLastSample();
+
+        Vector3f acceleration(data.accelerationX, data.accelerationY,
+                              data.accelerationZ);
+        Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                                 data.angularVelocityZ);
+        Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+        angularVelocity = angularVelocity - offset;
+        angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+        Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                               data.magneticFieldZ);
+        Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+        Matrix3f A{{0.688760050772712, 0, 0},
+                   {0, 0.637715211784480, 0},
+                   {0, 0, 2.27669720320908}};
+        magneticField = (magneticField - b).transpose() * A;
+
+        acceleration.normalize();
+        magneticField.normalize();
+
+        StateInitializer state;
+        state.triad(acceleration, magneticField, nedMag);
+
+        auto kalmanState = state.getInitX();
+        if (!std::isnan(kalmanState(9)))
+            printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+                   kalmanState(7), kalmanState(8));
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
+}
+
+void imuInit()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmx_config;
+    bmx_config.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark = 80;
+    bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    bmx_config.temperatureDivider = 1;
+
+    bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+
+    bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+
+    bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                     spiConfig);
+}
diff --git a/src/tests/catch/test-aero.cpp b/src/tests/catch/test-aero.cpp
index d9542136f7a385267efb7039f95738b92212b784..6e8ba3a2d5603a3910323cbff12097e014dbff84 100644
--- a/src/tests/catch/test-aero.cpp
+++ b/src/tests/catch/test-aero.cpp
@@ -24,7 +24,7 @@
 #include "catch-tests-entry.cpp"
 #endif
 
-#include <utils/aero/AeroUtils.h>
+#include <utils/AeroUtils/AeroUtils.h>
 
 #include <catch2/catch.hpp>
 
@@ -32,7 +32,7 @@ using namespace Boardcore;
 
 TEST_CASE("[AeroUtils] mslTemperature")
 {
-    using namespace aeroutils;
+    using namespace Aeroutils;
     Approx isa_T0 =
         Approx(288.151).margin(0.001);  // 15 deg celsius, 0.01% error allowed
 
@@ -46,7 +46,7 @@ TEST_CASE("[AeroUtils] mslTemperature")
 
 TEST_CASE("[AeroUtils] mslPressure")
 {
-    using namespace aeroutils;
+    using namespace Aeroutils;
     Approx isa_P0 =
         Approx(101325).epsilon(0.0001);  // 15 deg celsius, 0.01% error allowed
 
@@ -61,7 +61,7 @@ TEST_CASE("[AeroUtils] mslPressure")
 float mslAltitude(float pressure, float pressureRef, float temperatureRef,
                   float zRef)
 {
-    using namespace aeroutils;
+    using namespace Aeroutils;
     float t0 = mslTemperature(temperatureRef, zRef);
 
     return relAltitude(pressure, mslPressure(pressureRef, temperatureRef, zRef),
@@ -84,7 +84,7 @@ TEST_CASE("[AeroUtils] relAltitude")
 
 TEST_CASE("[AeroUtils] relDensity")
 {
-    using namespace aeroutils;
+    using namespace Aeroutils;
 
     REQUIRE(relDensity(101325, 101325, 0, 288.15) ==
             Approx(1.225).epsilon(0.0001));
@@ -100,7 +100,7 @@ TEST_CASE("[AeroUtils] relDensity")
 
 TEST_CASE("[AeroUtils] verticalSpeed")
 {
-    using namespace aeroutils;
+    using namespace Aeroutils;
 
     const int count = 5;
     float p[]       = {100129.4, 99555.8, 89153.1, 23611.1, 101284.6};
diff --git a/src/tests/catch/test-eventbroker.cpp b/src/tests/catch/test-eventbroker.cpp
index 3139a50ed78563fb01404f22ab88ae780139a2ad..18a9eb65c348d484a29c2c7823be366e4cc042d0 100644
--- a/src/tests/catch/test-eventbroker.cpp
+++ b/src/tests/catch/test-eventbroker.cpp
@@ -29,7 +29,7 @@
 
 #include <events/EventBroker.h>
 #include <miosix.h>
-#include <utils/testutils/TestHelper.h>
+#include <utils/TestUtils/TestHelper.h>
 
 #include <catch2/catch.hpp>
 #include <cstdio>
@@ -75,26 +75,26 @@ TEST_CASE("EventBroker - Posts to different topics")
 
     SECTION("Post event on TOPIC_1, only sub1 should receive it")
     {
-        ev.code = EV_A;
+        ev = EV_A;
         broker.post(ev, TOPIC_1);
         REQUIRE(sub1.getTotalCount() == 1);
-        REQUIRE(sub1.getLastEvent() == ev.code);
+        REQUIRE(sub1.getLastEvent() == ev);
         REQUIRE(sub2.getTotalCount() == 0);
     }
 
     SECTION("Post event on TOPIC_2, both sub1 and sub2 should receive it")
     {
-        ev.code = EV_B;
+        ev = EV_B;
         broker.post(ev, TOPIC_2);
         REQUIRE(sub1.getTotalCount() == 1);
         REQUIRE(sub2.getTotalCount() == 1);
-        REQUIRE(sub1.getLastEvent() == ev.code);
-        REQUIRE(sub2.getLastEvent() == ev.code);
+        REQUIRE(sub1.getLastEvent() == ev);
+        REQUIRE(sub2.getLastEvent() == ev);
     }
 
     SECTION("Post event on TOPIC_3, no one should receive it")
     {
-        ev.code = EV_C;
+        ev = EV_C;
         broker.post(ev, TOPIC_3);
 
         REQUIRE(sub1.getTotalCount() == 0);
diff --git a/src/tests/catch/test-kalman-eigen.cpp b/src/tests/catch/test-kalman-eigen.cpp
index fc2245c23deaeb8ca49337ab5afee9796fb2d642..71974bebedf31ff6790e7cc7104c9c76b5fd2538 100644
--- a/src/tests/catch/test-kalman-eigen.cpp
+++ b/src/tests/catch/test-kalman-eigen.cpp
@@ -26,12 +26,12 @@
 
 #define EIGEN_RUNTIME_NO_MALLOC
 
-#include <algorithms/kalman/KalmanEigen.h>
+#include <algorithms/Kalman/Kalman.h>
 
 #include <catch2/catch.hpp>
 #include <iostream>
 
-#include "../kalman/test-kalman-data.h"
+#include "../algorithms/Kalman/test-kalman-data.h"
 
 using namespace Boardcore;
 using namespace Eigen;
@@ -61,10 +61,10 @@ static const Matrix<float, OUTPUTS_DIM, OUTPUTS_DIM> R{10};
 // State vector
 static const Matrix<float, STATES_DIM, 1> x0(INPUT[0], 0.0, 0.0);
 
-static const KalmanEigen<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig
+static const Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig
 getKalmanConfig()
 {
-    KalmanEigen<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig config;
+    Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig config;
     config.F = F;
     config.H = H;
     config.Q = Q;
@@ -77,7 +77,7 @@ getKalmanConfig()
 
 TEST_CASE("Update test")
 {
-    KalmanEigen<float, STATES_DIM, OUTPUTS_DIM> filter(getKalmanConfig());
+    Kalman<float, STATES_DIM, OUTPUTS_DIM> filter(getKalmanConfig());
 
     Matrix<float, OUTPUTS_DIM, 1> y{};
     float lastTime = TIME[0];
diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp
index 6d09548dc20e7b2cf7323c25d9b654919033c2c8..e0cf2865ac8da3f9dbc691dbcd583c2aaa79ec03 100644
--- a/src/tests/catch/test-packetqueue.cpp
+++ b/src/tests/catch/test-packetqueue.cpp
@@ -82,10 +82,12 @@ TEST_CASE("Packet tests")
 
     SECTION("Adding stuff to packet")
     {
+        // Add 5 bytes
+        REQUIRE(p.append(messageBase, 5));
+        uint64_t ts = p.getTimestamp();
 
-        REQUIRE(p.tryAppend(messageBase, 5));
-        uint64_t ts = p.timestamp();
-        REQUIRE(miosix::getTick() - ts < 5);
+        REQUIRE(Boardcore::TimestampTimer::getInstance().getTimestamp() - ts <
+                5);
         REQUIRE(p.dump(buf) == 5);
         COMPARE(buf, BUF_LEN, "01234");
 
@@ -93,29 +95,21 @@ TEST_CASE("Packet tests")
         REQUIRE(p.size() == 5);
         REQUIRE(p.getMsgCount() == 1);
 
-        REQUIRE(p.tryAppend(messageBase + 5, 3));
+        // Add 3 bytes
+        REQUIRE(p.append(messageBase + 5, 3));
         REQUIRE(p.dump(buf) == 8);
         COMPARE(buf, BUF_LEN, "01234567");
         REQUIRE(p.isEmpty() == false);
         REQUIRE(p.size() == 8);
 
-        REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3));
-        REQUIRE(p.dump(buf) == 8);
-        COMPARE(buf, BUF_LEN, "01234567");
-        REQUIRE(p.isEmpty() == false);
-        REQUIRE(p.size() == 8);
-        REQUIRE(p.getMsgCount() == 2);
-
-        REQUIRE(p.tryAppend(messageBase + 8, 2));
-        REQUIRE(p.dump(buf) == 10);
+        // Trying to add 3 more bytes, only 2 should be written
+        REQUIRE(p.append(messageBase + 8, 3) == 2);
+        REQUIRE(p.dump(buf) == PKT_LEN);
         COMPARE(buf, BUF_LEN, "0123456789");
         REQUIRE(p.isEmpty() == false);
-        REQUIRE(p.isFull());
-        REQUIRE(p.size() == 10);
+        REQUIRE(p.size() == PKT_LEN);
         REQUIRE(p.getMsgCount() == 3);
 
-        REQUIRE(p.timestamp() == ts);
-
         p.clear();
         REQUIRE(p.isEmpty());
         REQUIRE(p.isFull() == false);
@@ -130,7 +124,7 @@ TEST_CASE("Packet tests")
     SECTION("Edge cases")
     {
         INFO("Adding empty msg");
-        REQUIRE_FALSE(p.tryAppend(messageBase, 0));
+        REQUIRE_FALSE(p.append(messageBase, 0));
         REQUIRE(p.isEmpty());
         REQUIRE(p.isFull() == false);
         REQUIRE(p.isReady() == false);
@@ -141,29 +135,16 @@ TEST_CASE("Packet tests")
         REQUIRE(p.dump(buf) == 0);
 
         INFO("Adding too big msg");
-        REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1));
-
-        REQUIRE(p.isEmpty());
-        REQUIRE(p.isFull() == false);
-        REQUIRE(p.isReady() == false);
-        REQUIRE(p.size() == 0);
-        REQUIRE(p.maxSize() == PKT_LEN);
+        REQUIRE(p.append(messageBase, PKT_LEN + 1) == PKT_LEN);
 
-        REQUIRE(p.getMsgCount() == 0);
-        REQUIRE(p.dump(buf) == 0);
-
-        INFO("Adding something to full packet")
-        REQUIRE(p.tryAppend(messageBase, PKT_LEN));
-        REQUIRE_FALSE(p.tryAppend(messageBase, 1));
-
-        REQUIRE(p.isEmpty() == false);
+        REQUIRE_FALSE(p.isEmpty());
         REQUIRE(p.isFull());
-        REQUIRE(p.isReady() == false);
+        REQUIRE_FALSE(p.isReady());
         REQUIRE(p.size() == PKT_LEN);
         REQUIRE(p.maxSize() == PKT_LEN);
 
         REQUIRE(p.getMsgCount() == 1);
-        REQUIRE(p.dump(buf) == 10);
+        REQUIRE(p.dump(buf) == PKT_LEN);
         COMPARE(buf, 10, "0123456789");
     }
 }
@@ -182,100 +163,142 @@ TEST_CASE("PacketQueue tests")
     SECTION("Normal operation")
     {
         INFO("Adding two elements to first packet");
-        REQUIRE(pq.put(messageBase, 4) == 0);
-        REQUIRE(pq.put(messageBase, 4) == 0);
+        REQUIRE(pq.put(messageBase, 4));
+        REQUIRE(pq.put(messageBase, 4));
 
+        // No packet should be ready
         REQUIRE(pq.countReady() == 0);
         REQUIRE(pq.countNotEmpty() == 1);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
         INFO("Adding third element and filling first packet");
-        REQUIRE(pq.put(messageBase, 2) == 0);
+        REQUIRE(pq.put(messageBase, 2));
 
+        // Now one single packet should be filled and ready
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 1);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
+        // Check the packet content
         Packet<PKT_LEN> p = pq.get();
         REQUIRE(p.getMsgCount() == 3);
         REQUIRE(p.isFull());
         REQUIRE(p.isReady());
         COMPARE(p, "0123012301");
 
-        INFO("Adding element to second packet");
-        REQUIRE(pq.put(messageBase + 10, 4) == 0);
+        INFO("Adding more data to create a second packet");
+        REQUIRE(pq.put(messageBase + 10, 4));
 
+        // The second packet should not be ready
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 2);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
         REQUIRE_FALSE(pq.buffer.get(1).isReady());
-
         COMPARE(pq.buffer.get(1), "abcd");
 
         p = pq.get();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
 
-        INFO(
-            "Adding element not fitting the second packet, added to the third");
-        REQUIRE(pq.put(messageBase + 10, 7) == 0);
+        INFO("Adding more data to create a third packet");
+        REQUIRE(pq.put(messageBase + 10, 7));
+
         p = pq.get();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
 
-        REQUIRE(pq.countReady() == 2);
-        REQUIRE(pq.countNotEmpty() == 3);
-        REQUIRE_FALSE(pq.isEmpty());
-        REQUIRE_FALSE(pq.isFull());
-
+        // Check all the packages
         REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "0123012301");
         REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "abcdabcdef");
         REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "g");
 
-        COMPARE(pq.buffer.get(0), "0123012301");
-        COMPARE(pq.buffer.get(1), "abcd");
-        COMPARE(pq.buffer.get(2), "abcdefg");
+        // Check the queue stats
+        REQUIRE(pq.countReady() == 2);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE_FALSE(pq.isEmpty());
+        REQUIRE_FALSE(pq.isFull());
 
         INFO("Popping first element");
         p = pq.pop();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
         COMPARE(p, "0123012301");
 
-        // Should now return what was the second element
-        COMPARE(pq.get(), "abcd");
-
+        // The packets should now be shifted
         REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "abcdabcdef");
         REQUIRE_FALSE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == 1);
+        COMPARE(pq.buffer.get(1), "g");
 
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 2);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
-        INFO("Adding a msg back to the first packet and filling it");
-        REQUIRE(pq.put(messageBase, 10) == 0);
-        REQUIRE(pq.countReady() == 3);
+        INFO("Adding more data to fill the last packet");
+        REQUIRE(pq.put(messageBase, 10));
+        REQUIRE(pq.countReady() == 2);
         REQUIRE(pq.countNotEmpty() == 3);
         REQUIRE_FALSE(pq.isEmpty());
-        REQUIRE(pq.isFull());
+        REQUIRE_FALSE(pq.isFull());
 
-        COMPARE(pq.buffer.get(0), "abcd");
-        COMPARE(pq.buffer.get(1), "abcdefg");
-        COMPARE(pq.buffer.get(2), "0123456789");
+        // We should now have three packets
+        REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "abcdabcdef");
+        REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "g012345678");
+        REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "9");
+
+        // If we now add another 10 bytes the last packet, the last byte which
+        // does not fit should be put in a new packet at the start of the queue
+        REQUIRE(pq.put(messageBase, 10));
+        REQUIRE(pq.countReady() == 2);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE_FALSE(pq.isEmpty());
+        REQUIRE_FALSE(pq.isFull());
+        REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "g012345678");
+        REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "9012345678");
+        REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "9");
+
+        // And now by adding the last 9 bytes the queue should be marked ready
+        REQUIRE(pq.put(messageBase + 10, 9));
+        REQUIRE(pq.countReady() == 3);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(2), "9abcdefghi");
 
         INFO("Popping everything");
 
         p = pq.pop();
-        COMPARE(p, "abcd");
         REQUIRE(p.isReady());
+        REQUIRE(p.size() == PKT_LEN);
+        COMPARE(p, "g012345678");
 
         p = pq.pop();
-        COMPARE(p, "abcdefg");
+        COMPARE(p, "9012345678");
         REQUIRE(p.isReady());
 
         p = pq.pop();
-        COMPARE(p, "0123456789");
+        COMPARE(p, "9abcdefghi");
         REQUIRE(p.isReady());
 
         REQUIRE_FALSE(pq.isFull());
@@ -288,20 +311,20 @@ TEST_CASE("PacketQueue tests")
 
     SECTION("Edge cases")
     {
-        INFO("Adding too big msg")
-        REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1);
+        INFO("Adding too big msg");
+        REQUIRE_FALSE(pq.put(messageBase, PKT_LEN * QUEUE_LEN + 1));
         REQUIRE_FALSE(pq.isFull());
         REQUIRE(pq.isEmpty());
         REQUIRE(pq.countNotEmpty() == 0);
         REQUIRE(pq.countReady() == 0);
 
-        INFO("Adding empty message")
-        REQUIRE(pq.put(messageBase, 0) == -1);
+        INFO("Adding empty message");
+        REQUIRE_FALSE(pq.put(messageBase, 0));
 
-        INFO("Adding something to full queue")
-        REQUIRE(pq.put(messageBase, PKT_LEN) == 0);
-        REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0);
-        REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0);
+        INFO("Adding something to full queue");
+        REQUIRE(pq.put(messageBase, PKT_LEN));
+        REQUIRE(pq.put(messageBase + 5, PKT_LEN));
+        REQUIRE(pq.put(messageBase + 10, PKT_LEN));
 
         REQUIRE(pq.buffer.count() == 3);
         for (int i = 0; i < 3; i++)
@@ -319,7 +342,7 @@ TEST_CASE("PacketQueue tests")
         REQUIRE(pq.countNotEmpty() == 3);
         REQUIRE(pq.countReady() == 3);
 
-        INFO("Get/Pop on empty queue")
+        INFO("Get/Pop on empty queue");
         REQUIRE_NOTHROW(pq.pop());
         REQUIRE_NOTHROW(pq.pop());
         REQUIRE_NOTHROW(pq.pop());
diff --git a/src/tests/catch/test-sensormanager-catch.cpp b/src/tests/catch/test-sensormanager-catch.cpp
index 2a23d9685b18bf7ab0d08ade5367c9648a3e9896..1bead9c7c321e4110ac29a6a4c249340a98d9d8d 100644
--- a/src/tests/catch/test-sensormanager-catch.cpp
+++ b/src/tests/catch/test-sensormanager-catch.cpp
@@ -24,7 +24,7 @@
 #include "catch-tests-entry.cpp"
 #endif
 
-#include <utils/testutils/TestSensor.h>
+#include <utils/TestUtils/TestSensor.h>
 
 #include <catch2/catch.hpp>
 #include <iostream>
@@ -39,7 +39,7 @@ using namespace Boardcore;
 
 static const uint8_t FIRST_TASK_ID = 7;  // used to test IDs assignment to tasks
 
-class FailingSensor : public Sensor<TestData>
+class FailingSensorCatch : public Sensor<TestData>
 {
     bool init() { return true; }
 
@@ -122,7 +122,7 @@ private:
         /*Enabled=*/true};
 
     // always failing self-test
-    FailingSensor s5;
+    FailingSensorCatch s5;
     SensorInfo s5_info{
         /*ID=*/"s5",
         /*Period=*/2000,
diff --git a/src/tests/catch/xbee/MockXbeeSPIBus.h b/src/tests/catch/xbee/MockXbeeSPIBus.h
index d54b3167cdecfa1faf8514a19c8a854f5c6aaf1d..44c48d14175a5b647a9a60ffe97241f496fdfb91 100644
--- a/src/tests/catch/xbee/MockXbeeSPIBus.h
+++ b/src/tests/catch/xbee/MockXbeeSPIBus.h
@@ -24,8 +24,8 @@
 
 #include <radio/Xbee/APIFrameParser.h>
 #include <radio/Xbee/APIFrames.h>
-#include <utils/testutils/MockGpioPin.h>
-#include <utils/testutils/MockSPIBus.h>
+#include <utils/TestUtils/MockGpioPin.h>
+#include <utils/TestUtils/MockSPIBus.h>
 
 #include <deque>
 #include <functional>
diff --git a/src/tests/drivers/canbus/test-canbus-2way.cpp b/src/tests/drivers/canbus/test-canbus-2way.cpp
index 2178668e0ffa0a9d36734ff77cf5261abda7a7fd..ab566f0e4555090de6499ef6d35544dd73464bf1 100644
--- a/src/tests/drivers/canbus/test-canbus-2way.cpp
+++ b/src/tests/drivers/canbus/test-canbus-2way.cpp
@@ -26,6 +26,7 @@
 // which it will respond
 
 #include <utils/Debug.h>
+#include <utils/Stats/Stats.h>
 
 #include <string>
 
@@ -34,7 +35,6 @@
 #include "diagnostic/PrintLogger.h"
 #include "drivers/canbus/BusLoadEstimation.h"
 #include "drivers/canbus/Canbus.h"
-#include "math/Stats.h"
 #include "utils/collections/CircularBuffer.h"
 
 constexpr uint32_t BAUD_RATE         = 1000 * 1000;
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
index d484d39cab98c3703d89d27da0c176900cca7690..31db75f4f5c47aa0a847be004e403718df858116 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bench.cpp
+++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
@@ -93,9 +93,6 @@ struct Stats
 
 } stats;
 
-/// Interval between transmissions.
-const int TX_INTERVAL = 1;
-
 SX1278 *sx1278[2] = {nullptr, nullptr};
 
 struct Msg
@@ -115,12 +112,10 @@ void recvLoop(int idx)
     while (1)
     {
         Msg msg;
-        msg.idx     = 0;
-        msg.dummy_1 = 0;
-        msg.dummy_2 = 0;
-        msg.dummy_3 = 0;
+        memset(&msg, 0, sizeof(msg));
 
         int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg));
+
         if (len != sizeof(msg))
         {
             stats.recv_errors++;
@@ -143,8 +138,6 @@ void sendLoop(int idx)
 {
     while (1)
     {
-        miosix::Thread::sleep(TX_INTERVAL);
-
         int next_idx = stats.send_count + 1;
 
         Msg msg;
@@ -269,6 +262,7 @@ int main()
     std::thread recv([]() { recvLoop(0); });
 #endif
 #ifndef DISABLE_TX
+    miosix::Thread::sleep(500);
     std::thread send([]() { sendLoop(1); });
 #endif
 
diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
index 0078a16b949f927ca05b82cd7dd6f67b2023b40f..22df3fe51535b7a62aefe3bdce39aae66208ec5f 100644
--- a/src/tests/drivers/sx1278/test-sx1278-serial.cpp
+++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
@@ -31,17 +31,17 @@
 using namespace Boardcore;
 using namespace miosix;
 
-SPIBus bus(SPI3);
+SPIBus bus(SPI4);
 
-GpioPin sck(GPIOC_BASE, 10);
-GpioPin miso(GPIOC_BASE, 11);
-GpioPin mosi(GPIOC_BASE, 12);
-GpioPin cs(GPIOA_BASE, 1);
-GpioPin dio(GPIOC_BASE, 15);
+GpioPin sck(GPIOE_BASE, 2);
+GpioPin miso(GPIOE_BASE, 5);
+GpioPin mosi(GPIOE_BASE, 6);
+GpioPin cs(GPIOC_BASE, 1);
+GpioPin dio(GPIOF_BASE, 10);
 
 SX1278* sx1278 = nullptr;
 
-void __attribute__((used)) EXTI15_IRQHandlerImpl()
+void __attribute__((used)) EXTI10_IRQHandlerImpl()
 {
     if (sx1278)
         sx1278->handleDioIRQ();
@@ -54,16 +54,16 @@ void initBoard()
         miosix::FastInterruptDisableLock dLock;
 
         // Enable SPI3
-        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // Enable SPI4 bus
         RCC_SYNC();
 
         // Setup SPI pins
         sck.mode(miosix::Mode::ALTERNATE);
-        sck.alternateFunction(6);
+        sck.alternateFunction(5);
         miso.mode(miosix::Mode::ALTERNATE);
-        miso.alternateFunction(6);
+        miso.alternateFunction(5);
         mosi.mode(miosix::Mode::ALTERNATE);
-        mosi.alternateFunction(6);
+        mosi.alternateFunction(5);
 
         cs.mode(miosix::Mode::OUTPUT);
         dio.mode(miosix::Mode::INPUT);
@@ -77,30 +77,35 @@ void initBoard()
 void recvLoop()
 {
     uint8_t msg[256];
-    auto console = miosix::DefaultConsole::instance().get();
-
     while (1)
     {
         int len = sx1278->receive(msg, sizeof(msg));
         if (len > 0)
         {
-            console->writeBlock(msg, len, 0);
-            // TODO: Flushing?
+            auto serial = miosix::DefaultConsole::instance().get();
+            serial->writeBlock(msg, len, 0);
         }
     }
 }
 
 void sendLoop()
 {
-    uint8_t msg[256];
-    auto console = miosix::DefaultConsole::instance().get();
+    // I create a GPIO with the onboard led to tell the user that
+    // a package is being sent
+    miosix::GpioPin led(GPIOG_BASE, 13);
+    led.mode(miosix::Mode::OUTPUT);
+    led.low();
 
+    uint8_t msg[63];
     while (1)
     {
-        int len = console->readBlock(msg, sizeof(msg), 0);
+        auto serial = miosix::DefaultConsole::instance().get();
+        int len     = serial->readBlock(msg, sizeof(msg), 0);
         if (len > 0)
         {
+            led.high();
             sx1278->send(msg, len);
+            led.low();
         }
     }
 }
@@ -122,13 +127,20 @@ int main()
         return -1;
     }
 
+    printf("\n[sx1278] Initialization complete!\n");
+
     std::thread recv([]() { recvLoop(); });
     std::thread send([]() { sendLoop(); });
 
-    printf("\n[sx1278] Initialization complete!\n");
+    for (;;)
+    {
+        miosix::Thread::sleep(100);
+    }
 
-    while (1)
-        miosix::Thread::wait();
+    // God please forgive me
+    // FIXME(davide.mor): ABSOLUTELY fix this
+    // miosix::Thread::sleep(20000);
+    // miosix::reboot();
 
     return 0;
 }
diff --git a/src/tests/drivers/test-MBLoadCell.cpp b/src/tests/drivers/test-MBLoadCell.cpp
index 5993c26044a69a6a537e82b5effba75ffee69f74..f5d97bb0da552add766b54542ba97f45b014e32d 100644
--- a/src/tests/drivers/test-MBLoadCell.cpp
+++ b/src/tests/drivers/test-MBLoadCell.cpp
@@ -21,15 +21,16 @@
  */
 
 #include <diagnostic/PrintLogger.h>
-
-#include "sensors/MBLoadCell/MBLoadCell.h"
-#include "string.h"
-#include "utils/ButtonHandler.h"
+#include <sensors/MBLoadCell/MBLoadCell.h>
+#include <string.h>
+#include <utils/ButtonHandler/ButtonHandler.h>
 
 //#define PRINT_ALL_SAMPLES // To be defined if we want to print all the samples
 
 using namespace Boardcore;
 using namespace miosix;
+using namespace std;
+using namespace placeholders;
 
 using button            = miosix::Gpio<GPIOA_BASE, 0>;  ///< User button
 const uint8_t btnUserId = 1;
@@ -39,17 +40,17 @@ const uint8_t btnUserId = 1;
  * button is pressed for a long time, resets the minimum and maximum values of
  * the recorded weights
  */
-void buttonCallback(uint8_t btnId, ButtonPress btnPress, MBLoadCell *loadcell)
+void buttonCallback(ButtonEvent btnPress, MBLoadCell *loadcell)
 {
-    if (btnId == btnUserId && btnPress == ButtonPress::DOWN)
+    if (btnPress == ButtonEvent::PRESSED)
         TRACE(
             "## MAX: %.2f [Kg] (ts: %.3f)\t##\tMIN: %.2f [Kg] (ts: %.3f) ##\n",
-            loadcell->getMaxWeight().weight,
-            loadcell->getMaxWeight().weightTimestamp / 1000000.0,
-            loadcell->getMinWeight().weight,
-            loadcell->getMinWeight().weightTimestamp / 1000000.0);
+            loadcell->getMaxWeight().load,
+            loadcell->getMaxWeight().loadTimestamp / 1000000.0,
+            loadcell->getMinWeight().load,
+            loadcell->getMinWeight().load / 1000000.0);
 
-    if (btnId == btnUserId && btnPress == ButtonPress::LONG)
+    if (btnPress == ButtonEvent::LONG_PRESS)
         loadcell->resetMaxMinWeights();
 }
 
@@ -67,14 +68,9 @@ int main()
      */
     MBLoadCell loadCell(LoadCellModes::CONT_MOD_T, 2, 115200);
 
-    // Binding the load cell instance to the callback to be called
-    std::function<void(uint8_t, ButtonPress)> callback =
-        std::bind(buttonCallback, std::placeholders::_1, std::placeholders::_2,
-                  &loadCell);
-
     // Instanciating the button
-    ButtonHandler<button> btnHandler(btnUserId, callback);
-    btnHandler.start();
+    ButtonHandler::getInstance().registerButtonCallback(
+        button::getPin(), bind(buttonCallback, _1, &loadCell));
 
     // Initializing the load cell
     if (!loadCell.init())
diff --git a/src/tests/drivers/test-dsgamma.cpp b/src/tests/drivers/test-dsgamma.cpp
index 03c9555075b41cec6929157105bea79f95b9037a..1445ae8a674aaa467b3713faf1f8eef368ab9694 100644
--- a/src/tests/drivers/test-dsgamma.cpp
+++ b/src/tests/drivers/test-dsgamma.cpp
@@ -170,8 +170,10 @@ int main()
         }
 
         printf("Bytes received: %d\nDropped: %d,Time:%d ms\n", index, lostBytes,
+               // cppcheck-suppress uninitvar
                (int)(endT - startT));
         printf("Speed: %.3f KB/s\n",
+               // cppcheck-suppress uninitvar
                index / ((endT - startT) / 1024.0f) / 1024.0f);
         /*  for (int i = 0; i < index; i++)
           {
diff --git a/src/tests/drivers/usart/test-usart.cpp b/src/tests/drivers/usart/test-usart.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce99d3d5a5a764b696598f8b0bf2c1ef49eccd7f
--- /dev/null
+++ b/src/tests/drivers/usart/test-usart.cpp
@@ -0,0 +1,182 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <fmt/format.h>
+
+#include <cassert>
+
+#include "drivers/usart/USART.h"
+#include "miosix.h"
+#include "string"
+#include "string.h"
+#include "thread"
+#include "utils/SerialInterface.h"
+
+using namespace miosix;
+using namespace Boardcore;
+
+/**
+ * SETUP:
+ * - connect the default serial (USART3) to the pc
+ * - connect usartx_rx to the usarty_tx
+ * - connect usarty_rx to the usartx_tx
+ *
+ * WARNING: If using the STM32SerialWrapper to read, the test passes only if we:
+ *  1. do a write/writeString with USART or STM32SerialWrapper;
+ *  2. sleep 10ms or more (or a printf...)
+ *  3. we finally read from STM32SerialWrapper::read method.
+ * if we omit the waiting time we end up failing the test. If more data is sent,
+ * then more baudrates tests will fail (from 2400 to greater). The USART driver
+ * doesn't have this problem.
+ */
+
+typedef struct
+{
+    char dataChar;
+    int dataInt;
+    float dataFloat;
+    double dataDouble;
+
+    std::string print()
+    {
+        return fmt::format("{},{:d},{:f},{:f}", dataChar, dataInt, dataFloat,
+                           dataDouble);
+    }
+} StructToSend;
+StructToSend struct_tx = {'C', 42, 420.69, 48.84};
+char buf_tx[64]        = "Testing communication, but very very very loong :D";
+USARTInterface::Baudrate baudrates[] = {
+    USARTInterface::Baudrate::B2400,   USARTInterface::Baudrate::B9600,
+    USARTInterface::Baudrate::B19200,  USARTInterface::Baudrate::B38400,
+    USARTInterface::Baudrate::B57600,  USARTInterface::Baudrate::B115200,
+    USARTInterface::Baudrate::B230400, USARTInterface::Baudrate::B460800,
+    USARTInterface::Baudrate::B921600};
+
+/**
+ * Communication: src -> dst
+ * tests the writeString, write and read methods of the USART drivers
+ */
+bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst)
+{
+    char buf_rx[64];
+    StructToSend struct_rx;
+    bool passed = true;
+
+    /************************** SENDING STRING **************************/
+    printf("Sending string\n");
+
+    printf("\t%d--> sent: \t'%s'\n", src->getId(), buf_tx);
+    src->writeString(buf_tx);
+    // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper
+    dst->read(buf_rx, 64);
+
+    printf("\t%d<-- received: \t'%s'\n", dst->getId(), buf_rx);
+
+    if (strcmp(buf_tx, buf_rx) == 0)
+    {
+        printf("*** %d -> %d WORKING!\n", src->getId(), dst->getId());
+    }
+    else
+    {
+        printf("### %d -> %d ERROR!\n", src->getId(), dst->getId());
+        passed = false;
+    }
+
+    /*********************** SENDING BINARY DATA ************************/
+
+    printf("Sending binary data\n");
+    printf("\t%d--> sent: \t'%s'\n", src->getId(), struct_tx.print().c_str());
+    src->write(&struct_tx, sizeof(StructToSend));
+    // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper
+    dst->read(&struct_rx, sizeof(StructToSend));
+    printf("\t%d<-- received: \t'%s'\n", dst->getId(),
+           struct_rx.print().c_str());
+
+    if (memcmp(&struct_tx, &struct_rx, sizeof(StructToSend)) == 0)
+    {
+        printf("*** %d -> %d WORKING!\n", src->getId(), dst->getId());
+    }
+    else
+    {
+        printf("### %d -> %d ERROR!\n", src->getId(), dst->getId());
+        passed = false;
+    }
+
+    return passed;
+}
+
+/* Available default pins:
+ * - USART1: tx=PA9  rx=PA10
+ * - USART2: tx=PA2  rx=PA3
+ * - USART3: tx=PB10 rx=PB11
+ * - UART4:  tx=PA0 rx=PA1
+ * - UART5:  tx=PC12 rx=PD2
+ * - USART6: tx=PC6 rx=PC7
+ * - UART7: tx=PE8 rx=PE7
+ * - UART8: tx=PE1 rx=PE0
+ */
+int main()
+{
+    bool testPassed = true;
+    printf("*** SERIAL 3 WORKING!\n");
+    for (unsigned int iBaud = 0;
+         iBaud < sizeof(baudrates) / sizeof(baudrates[0]); iBaud++)
+    {
+        USARTInterface::Baudrate baudrate = baudrates[iBaud];
+        printf("\n\n########################### %d\n", (int)baudrate);
+
+        // declaring the usart peripherals
+        STM32SerialWrapper usartx(USART1, baudrate, u1rx2::getPin(),
+                                  u1tx1::getPin());
+        usartx.init();
+
+        USART usarty(UART4, baudrate);
+        // usarty.initPins(u5tx::getPin(), 8, u5rx::getPin(), 8);
+        // usarty.setOversampling(false);
+        // usarty.setStopBits(1);
+        // usarty.setWordLength(USART::WordLength::BIT8);
+        // usarty.setParity(USART::ParityBit::NO_PARITY);
+        usarty.init();
+
+        // testing transmission (both char and binary) "serial 1 <- serial 2"
+        testPassed &= testCommunicationSequential(&usartx, &usarty);
+
+        // testing transmission (both char and binary) "serial 1 -> serial 2"
+        testPassed &= testCommunicationSequential(&usarty, &usartx);
+    }
+
+    if (testPassed)
+    {
+        printf(
+            "********************************\n"
+            "***        TEST PASSED       ***\n"
+            "********************************\n");
+    }
+    else
+    {
+        printf(
+            "################################\n"
+            "###        TEST FAILED       ###\n"
+            "################################\n");
+    }
+    return 0;
+}
diff --git a/src/tests/drivers/xbee/XbeeTestData.h b/src/tests/drivers/xbee/XbeeTestData.h
index e7e6cc4c8f67fdce1f0488892b957bbfa7fe2db7..cde8e70c9d678bd10c26a2907de0d53b0dc59686 100644
--- a/src/tests/drivers/xbee/XbeeTestData.h
+++ b/src/tests/drivers/xbee/XbeeTestData.h
@@ -22,7 +22,7 @@
 
 #pragma once
 
-#include <math/Stats.h>
+#include <utils/Stats/Stats.h>
 
 #include <array>
 #include <ostream>
diff --git a/src/tests/drivers/xbee/XbeeTransceiver.h b/src/tests/drivers/xbee/XbeeTransceiver.h
index 049b9afe73c538d7db0245505ff6446ed2e30470..3e41850e557367880e47fde90bcd57fc11788527 100644
--- a/src/tests/drivers/xbee/XbeeTransceiver.h
+++ b/src/tests/drivers/xbee/XbeeTransceiver.h
@@ -28,7 +28,7 @@
 #include <radio/Xbee/APIFramesLog.h>
 #include <radio/Xbee/Xbee.h>
 #include <utils/Debug.h>
-#include <utils/testutils/ThroughputCalculator.h>
+#include <utils/TestUtils/ThroughputCalculator.h>
 
 #include <functional>
 
diff --git a/src/tests/drivers/xbee/gui/EnergyScanScreen.h b/src/tests/drivers/xbee/gui/EnergyScanScreen.h
index 081d375486f11fba2cf4a95af1e08f81d248e069..49259f2d8967279274d3adca993b0bdaec713a8b 100644
--- a/src/tests/drivers/xbee/gui/EnergyScanScreen.h
+++ b/src/tests/drivers/xbee/gui/EnergyScanScreen.h
@@ -22,8 +22,8 @@
 
 #pragma once
 
-#include <math/Stats.h>
 #include <mxgui/display.h>
+#include <utils/Stats/Stats.h>
 #include <utils/gui/GridLayout.h>
 #include <utils/gui/TextView.h>
 #include <utils/gui/VerticalLayout.h>
diff --git a/src/tests/drivers/xbee/gui/StatusScreen.h b/src/tests/drivers/xbee/gui/StatusScreen.h
index 19cc9e6e9b8e4224c177b60da5f6299762f82d2b..b397715e0d772a8261ddfbb42f127b320ac9b848 100644
--- a/src/tests/drivers/xbee/gui/StatusScreen.h
+++ b/src/tests/drivers/xbee/gui/StatusScreen.h
@@ -24,11 +24,11 @@
 
 #include <logger/Logger.h>
 #include <mxgui/display.h>
+#include <utils/TestUtils/ThroughputCalculator.h>
 #include <utils/gui/GridLayout.h>
 #include <utils/gui/OptionView.h>
 #include <utils/gui/TextView.h>
 #include <utils/gui/VerticalLayout.h>
-#include <utils/testutils/ThroughputCalculator.h>
 
 #include <cstdint>
 #include <cstring>
diff --git a/src/tests/drivers/xbee/test-xbee-gui.cpp b/src/tests/drivers/xbee/test-xbee-gui.cpp
index 7c51a4003d40d767dfc4974177cc431283ee553b..407f90a1dd4c9682e45c5aa504388544b83ea0b0 100644
--- a/src/tests/drivers/xbee/test-xbee-gui.cpp
+++ b/src/tests/drivers/xbee/test-xbee-gui.cpp
@@ -27,7 +27,7 @@
 #include <radio/Xbee/APIFramesLog.h>
 #include <radio/Xbee/ATCommands.h>
 #include <radio/Xbee/Xbee.h>
-#include <utils/ButtonHandler.h>
+#include <utils/ButtonHandler/ButtonHandler.h>
 
 #include <array>
 #include <cstdio>
@@ -75,7 +75,6 @@ Xbee::Xbee* xbee = nullptr;
 ConstSendInterval sndInt{0};
 XbeeTransceiver* trans = nullptr;
 XbeeGUI* gui;
-ButtonHandler<GpioUserBtn>* btnHandler;
 
 unsigned int markCounter = 1;
 
@@ -145,10 +144,9 @@ int main()
     // GUI
     gui = new XbeeGUI();
 
-    btnHandler = new ButtonHandler<GpioUserBtn>(
-        0, bind(&ScreenManager::onButtonPress, &gui->screenManager, _2));
-
-    btnHandler->start();
+    ButtonHandler::getInstance().registerButtonCallback(
+        GpioUserBtn::getPin(),
+        bind(&ScreenManager::onButtonEvent, &gui->screenManager, _1));
 
     gui->screenConfig.btnStart.addOnInteractionListener(onStartButtonClick);
     gui->screenConfig.btnEnergy.addOnInteractionListener(onEnergyButtonClick);
diff --git a/src/tests/events/fsm/test-fsm.cpp b/src/tests/events/fsm/test-fsm.cpp
index 7c119cc16ddddbd6a58914e4f8f422a59a60ac09..b6f2dc4f7e3a8dfd43f198962d74abe988ee9c60 100644
--- a/src/tests/events/fsm/test-fsm.cpp
+++ b/src/tests/events/fsm/test-fsm.cpp
@@ -33,17 +33,18 @@ int main()
 {
     FSMExample fsm;
 
-    sEventBroker.start();  // Start broker thread
-    fsm.start();           // Start FSM thread
+    EventBroker::getInstance().start();  // Start broker thread
+    fsm.start();                         // Start FSM thread
 
     // State machine starts in state S1. Post EV_A to move to S2
-    sEventBroker.post(Event{EV_A}, TOPIC_T1);
+    EventBroker::getInstance().post(Event{EV_A}, TOPIC_T1);
 
     // FSM now in State S2
-    sEventBroker.post(Event{EV_E},
-                      TOPIC_T1);  // This makes the FSM print hello world
+    EventBroker::getInstance().post(
+        Event{EV_E},
+        TOPIC_T1);  // This makes the FSM print hello world
 
-    sEventBroker.post(Event{EV_C}, TOPIC_T1);  // Transition to S3
+    EventBroker::getInstance().post(Event{EV_C}, TOPIC_T1);  // Transition to S3
 
     printf("Waiting for the FSM to transition to S1\n");
     Thread::sleep(1100);
@@ -53,7 +54,7 @@ int main()
 
     // Since previously we've been in state S3, now v == 1 and EV_A will make
     // the FSM transition to S4 instead of S1
-    sEventBroker.post(Event{EV_A}, TOPIC_T1);
+    EventBroker::getInstance().post(Event{EV_A}, TOPIC_T1);
 
     // Now the state machine is in state S4
 
@@ -68,5 +69,5 @@ int main()
     // Stop the threds, even though we will never reach this point, but just for
     // correctness ;)
     fsm.stop();
-    sEventBroker.stop();
+    EventBroker::getInstance().stop();
 }
diff --git a/src/tests/events/fsm/test-fsm.h b/src/tests/events/fsm/test-fsm.h
index 7b16d6703e9cc8c6910dd012b2377d5461d7063c..f603cdfec09f72e09864f4dbeaf43e08264c9b6d 100644
--- a/src/tests/events/fsm/test-fsm.h
+++ b/src/tests/events/fsm/test-fsm.h
@@ -40,8 +40,8 @@ namespace Boardcore
  */
 enum ExampleEvents : uint8_t
 {
-    EV_A = EV_FIRST_SIGNAL,  // The first event must always have
-                             // value EV_FIRST_SIGNAL
+    EV_A = EV_FIRST_CUSTOM,  // The first event must always have
+                             // value EV_FIRST_CUSTOM
     EV_B,  // Values for the following event can be manually specified or
            // assigned automatically
     EV_C,
@@ -64,7 +64,7 @@ enum ExampleTopics : uint8_t
  */
 void traceState(uint8_t state, const Event& ev)
 {
-    switch (ev.code)
+    switch (ev)
     {
         case EV_ENTRY:
         {
@@ -78,7 +78,7 @@ void traceState(uint8_t state, const Event& ev)
         }
         default:
         {
-            printf("(S%d) Received event %d\n", state, ev.code);
+            printf("(S%d) Received event %d\n", state, ev);
             break;
         }
     }
@@ -110,10 +110,10 @@ public:
     FSMExample() : FSM(&FSMExample::state_S1), v(0)
     {
         // Subscribe for events posted on TOPIC_T1
-        sEventBroker.subscribe(this, TOPIC_T1);
+        EventBroker::getInstance().subscribe(this, TOPIC_T1);
     }
 
-    ~FSMExample() { sEventBroker.unsubscribe(this); }
+    ~FSMExample() { EventBroker::getInstance().unsubscribe(this); }
 
 private:
     /*
@@ -129,7 +129,7 @@ private:
         // state machine on the terminal.
         traceState(STATE_S1, ev);
 
-        switch (ev.code)
+        switch (ev)
         {
             // It's always good to add braces to every single case statement, to
             // avoid problems
@@ -169,7 +169,7 @@ private:
     {
         traceState(STATE_S2, ev);
 
-        switch (ev.code)
+        switch (ev)
         {
             case EV_ENTRY:
             {
@@ -200,7 +200,7 @@ private:
     {
         traceState(STATE_S3, ev);
 
-        switch (ev.code)
+        switch (ev)
         {
             case EV_ENTRY:
             {
@@ -208,15 +208,15 @@ private:
                 v = 1;
 
                 // Post EV_D to itself in 1 seconds
-                delayed_ev_id =
-                    sEventBroker.postDelayed<1000>(Event{EV_D}, TOPIC_T1);
+                delayed_ev_id = EventBroker::getInstance().postDelayed<1000>(
+                    Event{EV_D}, TOPIC_T1);
 
                 break;
             }
             case EV_EXIT:
             {
                 // Remove the delayed event in case it has not fired yet
-                sEventBroker.removeDelayed(delayed_ev_id);
+                EventBroker::getInstance().removeDelayed(delayed_ev_id);
                 break;
             }
             case EV_B:
@@ -243,7 +243,7 @@ private:
     {
         traceState(STATE_S4, ev);
 
-        switch (ev.code)
+        switch (ev)
         {
             case EV_ENTRY:
             {
diff --git a/src/tests/sensors/analog/test-battery-voltage.cpp b/src/tests/sensors/analog/test-battery-voltage.cpp
index 1e2065433ee6f6da108ba4e28896aa31294ab755..3bc6d96fbbfb94fdfcad3c9dc762e4a0d27bb020 100644
--- a/src/tests/sensors/analog/test-battery-voltage.cpp
+++ b/src/tests/sensors/analog/test-battery-voltage.cpp
@@ -23,7 +23,7 @@
 #include <drivers/adc/InternalADC.h>
 #include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
-#include <sensors/analog/battery/BatteryVoltageSensor.h>
+#include <sensors/analog/BatteryVoltageSensor.h>
 #include <utils/Debug.h>
 
 using namespace Boardcore;
diff --git a/src/tests/sensors/analog/test-current-sensor.cpp b/src/tests/sensors/analog/test-current-sensor.cpp
index 5983e82b6a53b915bf9a63036c847c66c1e1b2a2..b223d9041c2fb626326adb5c3e5bd29cf2063163 100644
--- a/src/tests/sensors/analog/test-current-sensor.cpp
+++ b/src/tests/sensors/analog/test-current-sensor.cpp
@@ -24,7 +24,7 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <interfaces-impl/gpio_impl.h>
 #include <miosix.h>
-#include <sensors/analog/current/CurrentSensor.h>
+#include <sensors/analog/CurrentSensor.h>
 #include <utils/Debug.h>
 
 using namespace miosix;
diff --git a/src/tests/sensors/calibration/test-calibration-benchmark.cpp b/src/tests/sensors/calibration/test-calibration-benchmark.cpp
index 066536cf73f042b53047cc615ae3ab5c67070f9d..1ed6b0f81de257186fd9c1c858b8648fce157998 100644
--- a/src/tests/sensors/calibration/test-calibration-benchmark.cpp
+++ b/src/tests/sensors/calibration/test-calibration-benchmark.cpp
@@ -21,9 +21,9 @@
  */
 
 /*  ACCELEROMETER calibration: please set the chosen one to 1  */
-#define BIAS_CALIBRATION_LOAD_TEST 1
-#define SIX_PARAMETER_CALIBRATION_LOAD_TEST 0
-#define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST 0
+#define BIAS_CALIBRATION_LOAD_TEST
+// #define SIX_PARAMETER_CALIBRATION_LOAD_TEST
+// #define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
 
 /* Expressed in Hertz: valid values: 1 <= frequency <= 1000 */
 #define SAMPLE_FREQUENCY_LOAD_TEST 1000
@@ -35,7 +35,6 @@
 #include <sensors/calibration/BiasCalibration.h>
 #include <sensors/calibration/HardIronCalibration.h>
 #include <sensors/calibration/SixParameterCalibration.h>
-#include <sensors/calibration/SoftIronCalibration.h>
 #include <sensors/calibration/TwelveParameterCalibration.h>
 #include <utils/Debug.h>
 
@@ -47,17 +46,17 @@ volatile AccelerometerData testData;
 
 int main()
 {
-#if BIAS_CALIBRATION_LOAD_TEST
+#ifdef BIAS_CALIBRATION_LOAD_TEST
     BiasCorrector<AccelerometerData> corrector;
     TRACE("Using BIAS calibration model.\n");
 #endif
 
-#if SIX_PARAMETER_CALIBRATION_LOAD_TEST
+#ifdef SIX_PARAMETER_CALIBRATION_LOAD_TEST
     SixParameterCorrector<AccelerometerData> corrector;
     TRACE("Using SIX-PARAMETER calibration model.\n");
 #endif
 
-#if TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
+#ifdef TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
     TwelveParameterCorrector<AccelerometerData> corrector;
     TRACE("Using TWELVE-PARAMETER calibration model.\n");
 #endif
diff --git a/src/tests/sensors/calibration/test-calibration-stats.cpp b/src/tests/sensors/calibration/test-calibration-stats.cpp
index a52fabb0f3b7ff29e35d60cbf52b6de16b8d648f..40aa29ad31fb57621dc67c00d6c83193c4b6d30b 100644
--- a/src/tests/sensors/calibration/test-calibration-stats.cpp
+++ b/src/tests/sensors/calibration/test-calibration-stats.cpp
@@ -23,13 +23,14 @@
 #define SENSOR_LIS3DSH_STATS_TEST 1
 
 #include <drivers/spi/SPIDriver.h>
-#include <math/Stats.h>
 #include <miosix.h>
+#include <utils/Stats/Stats.h>
 
 #if SENSOR_LIS3DSH_STATS_TEST
 #include <sensors/LIS3DSH/LIS3DSH.h>
 #endif
 
+#include <sensors/calibration/AxisOrientation.h>
 #include <sensors/calibration/Calibration.h>
 
 using namespace Boardcore;
diff --git a/src/tests/sensors/calibration/test-calibration.cpp b/src/tests/sensors/calibration/test-calibration.cpp
index 4172dec1c3bface25671de10f8f8a3f9479c511f..9e2429572ddd71e14b969bb8c10f77d473e9a619 100644
--- a/src/tests/sensors/calibration/test-calibration.cpp
+++ b/src/tests/sensors/calibration/test-calibration.cpp
@@ -39,7 +39,6 @@
 #include <sensors/calibration/BiasCalibration.h>
 #include <sensors/calibration/HardIronCalibration.h>
 #include <sensors/calibration/SixParameterCalibration.h>
-#include <sensors/calibration/SoftIronCalibration.h>
 #include <sensors/calibration/TwelveParameterCalibration.h>
 #include <utils/Debug.h>
 
diff --git a/src/tests/sensors/test-hx711.cpp b/src/tests/sensors/test-hx711.cpp
index 53e6622f98b409019717fc2ad72ee8a71ce5c89d..f4acb0d76fc4e5c9dee08a4b8a6477bb478a192c 100644
--- a/src/tests/sensors/test-hx711.cpp
+++ b/src/tests/sensors/test-hx711.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -54,19 +54,19 @@ int main()
     for (int i = 0; i < 100; i++)
     {
         sensor.sample();
-        average += sensor.getLastSample().weight;
+        average += sensor.getLastSample().load;
         Thread::sleep(12);
     }
     average /= 100;
-    sensor.setZero(-average);
+    sensor.setOffset(average);
     sensor.setScale(214000);
 
     while (true)
     {
         sensor.sample();
 
-        printf("[%.1f] %f\n", sensor.getLastSample().weightTimestamp / 1e6,
-               sensor.getLastSample().weight);
+        printf("[%.1f] %f\n", sensor.getLastSample().loadTimestamp / 1e6,
+               sensor.getLastSample().load);
 
         Thread::sleep(10);
     }
diff --git a/src/tests/sensors/test-l3gd20-fifo.cpp b/src/tests/sensors/test-l3gd20-fifo.cpp
index 31208a2f48ff4754786a88ab7b99eea51c8b0a57..2b6d46286f8df3ff7f3b620eb4e0a04860642da4 100644
--- a/src/tests/sensors/test-l3gd20-fifo.cpp
+++ b/src/tests/sensors/test-l3gd20-fifo.cpp
@@ -80,7 +80,7 @@ static constexpr unsigned int FIFO_WATERMARK = 12;
 // output data rate) is the following:
 static constexpr float SAMPLE_FREQUENCY = 782.3f;
 
-struct GyroSample
+struct GyroSampleFifo
 {
     int fifoNum;
     L3GD20Data gyro;
@@ -93,7 +93,7 @@ struct GyroSample
 // How many samples to collect
 static constexpr int NUM_SAMPLES = SAMPLE_FREQUENCY * 20;
 
-GyroSample data[NUM_SAMPLES];
+GyroSampleFifo data[NUM_SAMPLES];
 int dataCounter = 0;
 
 // Last interrupt received timer tick
diff --git a/src/tests/sensors/test-max31855.cpp b/src/tests/sensors/test-max31855.cpp
index 1c6527d4ed539e1f7668a709cf3a5c5f14caa099..00171ebab0da1cb58c04885821dc96e0b7526754 100644
--- a/src/tests/sensors/test-max31855.cpp
+++ b/src/tests/sensors/test-max31855.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -27,15 +27,19 @@
 using namespace miosix;
 using namespace Boardcore;
 
-GpioPin sckPin  = GpioPin(GPIOE_BASE, 2);
-GpioPin misoPin = GpioPin(GPIOE_BASE, 5);
-GpioPin csPin   = GpioPin(GPIOE_BASE, 4);
+GpioPin sckPin  = GpioPin(GPIOF_BASE, 7);
+GpioPin misoPin = GpioPin(GPIOF_BASE, 8);
+GpioPin csPin   = GpioPin(GPIOD_BASE, 13);
+
+GpioPin csMems = GpioPin(GPIOC_BASE, 1);
 
 void initBoard()
 {
     // Setup gpio pins
     csPin.mode(Mode::OUTPUT);
     csPin.high();
+    csMems.mode(Mode::OUTPUT);
+    csMems.high();
     sckPin.mode(Mode::ALTERNATE);
     sckPin.alternateFunction(5);
     misoPin.mode(Mode::ALTERNATE);
@@ -47,7 +51,7 @@ int main()
     // Enable SPI clock and set gpios
     initBoard();
 
-    SPIBus spiBus(SPI4);
+    SPIBus spiBus(SPI5);
     MAX31855 sensor{spiBus, csPin};
 
     printf("Starting process verification!\n");
@@ -62,7 +66,8 @@ int main()
         sensor.sample();
         TemperatureData sample = sensor.getLastSample();
 
-        printf("%.2f\n", sample.temperature);
+        printf("[%.2f] %.2f\n", sample.temperatureTimestamp / 1e6,
+               sample.temperature);
 
         Thread::sleep(500);
     }
diff --git a/src/tests/sensors/test-mpu9250.cpp b/src/tests/sensors/test-mpu9250.cpp
index ba2ad4ea4f8d1687c18554f12cacf41fa892ff35..03e798fa2ac28a1d7c23b334f8c715b67d2db0ae 100644
--- a/src/tests/sensors/test-mpu9250.cpp
+++ b/src/tests/sensors/test-mpu9250.cpp
@@ -53,18 +53,11 @@ void initBoard()
 int main()
 {
     // Enable SPI clock and set gpios
-    initBoard();
-
-    // SPI configuration setup
-
-    SPIBusConfig spiConfig;
-    spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
-    spiConfig.mode         = SPI::Mode::MODE_3;
-    SPIBus spiBus(SPI2);
-    SPISlave spiSlave(spiBus, csPin, spiConfig);
+    // initBoard();
 
     // Device initialization
-    MPU9250 mpu9250(spiSlave);
+    SPIBus spiBus(SPI1);
+    MPU9250 mpu9250(spiBus, sensors::mpu9250::cs::getPin());
 
     // Initialize the device
     bool result = mpu9250.init();
@@ -85,7 +78,7 @@ int main()
         printf("%lld,%f,%f,%f\n", data.magneticFieldTimestamp,
                data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
 
-        // Serial communicaion at 115200 baud takes aprx. 10ms
+        // Serial communication at 115200 baud takes approximately 10ms
         // miosix::delayMs(10);
     }
 
diff --git a/src/tests/sensors/test-ms5803.cpp b/src/tests/sensors/test-ms5803.cpp
index f3a8bcc1539d5b4eb9a7255f9056a85d043f6fec..4c00318899a0fd596e002c0baa3e350203ecd876 100644
--- a/src/tests/sensors/test-ms5803.cpp
+++ b/src/tests/sensors/test-ms5803.cpp
@@ -35,12 +35,9 @@ using namespace miosix;
 
 int main()
 {
-    SPIBusConfig spiConfig;
+    // Sample temperature every 10 pressure samples
     SPIBus spiBus(SPI1);
-    SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiConfig);
-
-    // Sample temperature every 5 pressure samples
-    MS5803 sensor(spiSlave, 10);
+    MS5803 sensor(spiBus, miosix::sensors::ms5803::cs::getPin(), {}, 10);
 
     Thread::sleep(100);
 
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..24b6cc092a73e531765fbbe4897a29133a827014
--- /dev/null
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -0,0 +1,91 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Davide Bonomini, Davide Mor, 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 <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/Debug.h>
+
+#include <cstdio>
+
+using namespace Boardcore;
+using namespace miosix;
+
+#define RATE 4
+
+int main()
+{
+    (void)TimestampTimer::getInstance();
+
+    printf("Welcome to the ublox test\n");
+
+    // Keep GPS baud rate at default for easier testing
+    UbloxGPS gps(921600, RATE, 2, "gps", 38400);
+    UbloxGPSData dataGPS;
+    printf("Gps allocated\n");
+
+    // Init the gps
+    if (gps.init())
+    {
+        printf("Successful gps initialization\n");
+    }
+    else
+    {
+        printf("Failed gps initialization\n");
+    }
+
+    // Perform the selftest
+    if (gps.selfTest())
+    {
+        printf("Successful gps selftest\n");
+    }
+    else
+    {
+        printf("Failed gps selftest\n");
+    }
+
+    // Start the gps thread
+    gps.start();
+    printf("Gps started\n");
+
+    while (true)
+    {
+        printf("a\n");
+        // Give time to the thread
+        Thread::sleep(1000 / RATE);
+
+        // Sample
+        gps.sample();
+        printf("b\n");
+        dataGPS = gps.getLastSample();
+
+        // Print out the latest sample
+        printf(
+            "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
+            "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
+            "track %3.1f\n",
+            (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix,
+            dataGPS.latitude, dataGPS.longitude, dataGPS.height,
+            dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth,
+            dataGPS.velocityEast, dataGPS.track);
+    }
+}
diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100.cpp
index 6d3c0bdbf7762840613c311729d25a08e6d70ada..df1d1870ec448660c8fe809f3cf3fb41783c7be3 100644
--- a/src/tests/sensors/test-vn100.cpp
+++ b/src/tests/sensors/test-vn100.cpp
@@ -29,19 +29,11 @@ using namespace Boardcore;
 
 int main()
 {
-    GpioPin tx(GPIOB_BASE, 6);
-    GpioPin rx(GPIOB_BASE, 7);
     VN100Data sample;
     string sampleRaw;
-    VN100 sensor{1, VN100::BaudRates::Baud_921600,
+    VN100 sensor{USART1, USARTInterface::Baudrate::B921600,
                  VN100::CRCOptions::CRC_ENABLE_16};
 
-    tx.mode(Mode::ALTERNATE);
-    rx.mode(Mode::ALTERNATE);
-
-    tx.alternateFunction(7);
-    rx.alternateFunction(7);
-
     // Let the sensor start up
     Thread::sleep(1000);
 
@@ -64,18 +56,19 @@ int main()
     // Sample and print 100 samples
     for (int i = 0; i < 100; i++)
     {
-        /*sensor.sample();
+        sensor.sample();
         sample = sensor.getLastSample();
-        //printf("%" PRIu64 ", %.3f, %.3f, %.3f\n",
-        sample.accelerationTimestamp, sample.accelerationX,
-        sample.accelerationY, sample.accelerationZ);
-        //printf("%.3f, %.3f, %.3f\n", sample.angularVelocityX,
-        sample.angularVelocityY, sample.angularVelocityZ);*/
+        printf("acc: %" PRIu64 ", %.3f, %.3f, %.3f\n",
+               sample.accelerationTimestamp, sample.accelerationX,
+               sample.accelerationY, sample.accelerationZ);
+        printf("ang: %.3f, %.3f, %.3f\n", sample.angularVelocityX,
+               sample.angularVelocityY, sample.angularVelocityZ);
 
         sensor.sampleRaw();
         sampleRaw = sensor.getLastRawSample();
         printf("%s\n", sampleRaw.c_str());
         // Thread::sleep(100);
+        printf("\n");
     }
 
     sensor.closeAndReset();
diff --git a/src/tests/test-eventinjector.cpp b/src/tests/test-eventinjector.cpp
index 424637b10027b701572790948541c489e6071e10..f36f11ba2a9bbfb3d0524d4369e69df63849fddc 100644
--- a/src/tests/test-eventinjector.cpp
+++ b/src/tests/test-eventinjector.cpp
@@ -37,7 +37,7 @@ int main()
     EventInjector injector;
     injector.start();
 
-    EventCounter counter(sEventBroker);
+    EventCounter counter(EventBroker::getInstance());
     counter.subscribe({topic});
 
     for (;;)
diff --git a/src/tests/test-hsm.cpp b/src/tests/test-hsm.cpp
index c23c9d7ef15384575ee2231e2e2d2f3cabe79b56..c33ba46ba6bffbb54c97b8682b79665d4b3275ef 100644
--- a/src/tests/test-hsm.cpp
+++ b/src/tests/test-hsm.cpp
@@ -44,7 +44,7 @@ using namespace miosix;
     {                                                              \
         cout << "------------------------------" << endl;          \
         cout << "Triggering signal " << #SIGNAL << endl;           \
-        sEventBroker.post({SIGNAL}, TOPIC_TEST);                   \
+        EventBroker::getInstance().post({SIGNAL}, TOPIC_TEST);     \
         Thread::sleep(400);                                        \
         testValue = HSM.testState(STATE);                          \
         cout << "Check State " << #STATE << " "                    \
@@ -54,7 +54,7 @@ using namespace miosix;
 
 enum TestEvents : uint8_t
 {
-    EV_A = EV_FIRST_SIGNAL,
+    EV_A = EV_FIRST_CUSTOM,
     EV_B,
     EV_C,
     EV_D,
@@ -84,14 +84,13 @@ public:
     bool foo;
 };
 
-#define DEBUG_PRINT \
-    cout << __func__ << ": event received:" << (int)e.code << endl
+#define DEBUG_PRINT cout << __func__ << ": event received:" << (int)e << endl
 
 using namespace std;
 
 HSMUTTest::HSMUTTest() : HSM(&HSMUTTest::state_initialization)
 {
-    sEventBroker.subscribe(this, TOPIC_TEST);
+    EventBroker::getInstance().subscribe(this, TOPIC_TEST);
 }
 
 State HSMUTTest::state_initialization(const Event& e)
@@ -105,7 +104,7 @@ State HSMUTTest::state_S(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -138,7 +137,7 @@ State HSMUTTest::state_S1(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -184,7 +183,7 @@ State HSMUTTest::state_S11(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -220,7 +219,7 @@ State HSMUTTest::state_S2(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -256,7 +255,7 @@ State HSMUTTest::state_S21(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -281,7 +280,7 @@ State HSMUTTest::state_S211(const Event& e)
 {
     State retState = HANDLED;
     DEBUG_PRINT;
-    switch (e.code)
+    switch (e)
     {
         case EV_ENTRY:
             break;
@@ -309,7 +308,7 @@ State HSMUTTest::state_S211(const Event& e)
 int main()
 {
 
-    sEventBroker.start();
+    EventBroker::getInstance().start();
 
     HSMUTTest& hsm = HSMUTTest::getInstance();
     hsm.start();
diff --git a/src/tests/test-max485.cpp b/src/tests/test-max485.cpp
index c63a1695a82d48f48a8a3aebe420835a4af2cdf2..18bb9077befedf2d449ad8707bf85a740e3b6dce 100644
--- a/src/tests/test-max485.cpp
+++ b/src/tests/test-max485.cpp
@@ -37,10 +37,10 @@
  *
  *  WIRINGS:
  *  Max485 n1   |   stm32f407vg_discovery ser 1
- *      DI      |       PA9
+ *      DI      |       PA9 / PB6
  *      DE      |       PC9
  *      RE      |       PC8
- *      RO      |       PA10
+ *      RO      |       PA10 / PB7
  *      VCC     |       3.3/5 V
  *      GND     |       GND
  *
@@ -56,8 +56,6 @@
  *      A       |       A
  *      B       |       B
  *
- * WARNINGS:
- * - the baudrate has to be extremely low. "guaranteed" at 2400
  */
 
 #include "string.h"
@@ -77,7 +75,8 @@ using ctrlPin2_s2 = miosix::Gpio<GPIOC_BASE, 2>;
 
 char msg[64] = "Testing communication :D";
 char rcv[64];
-int baudrates[6] = {2400, 3600, 4800, 9600, 19200, 115200};
+int baudrates[] = {2400,   3600,   4800,   9600,  19200,
+                   115200, 230400, 460800, 921600};
 
 // function for the thread that has to read from serial
 void readSer(SerialInterface *s)
@@ -132,7 +131,8 @@ int main()
     }
 
     printf("*** SERIAL 3 WORKING!\n");
-    for (int iBaud = 0; iBaud < 6; iBaud++)
+    for (unsigned int iBaud = 0;
+         iBaud < sizeof(baudrates) / sizeof(baudrates[0]); iBaud++)
     {
         Thread::sleep(1000);
 
diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp
index 72560d47c3398fcacb0a5a143809b5e747024464..1a8b06648b7c60c46f31c461c5485285622577d1 100644
--- a/src/tests/test-sensormanager.cpp
+++ b/src/tests/test-sensormanager.cpp
@@ -25,7 +25,7 @@
 #include <sensors/Sensor.h>
 #include <sensors/SensorManager.h>
 #include <utils/Debug.h>
-#include <utils/testutils/TestSensor.h>
+#include <utils/TestUtils/TestSensor.h>
 
 #include <functional>
 #include <iostream>
diff --git a/src/tests/utils/test-buttonhandler.cpp b/src/tests/utils/test-buttonhandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a47acd7bf682edbd120dbb0aa92a84338b277561
--- /dev/null
+++ b/src/tests/utils/test-buttonhandler.cpp
@@ -0,0 +1,72 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <miosix.h>
+#include <utils/ButtonHandler/ButtonHandler.h>
+
+#include <functional>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace std;
+using namespace placeholders;
+
+GpioPin button1 = GpioPin(GPIOG_BASE, 10);
+GpioPin button2 = GpioPin(GPIOG_BASE, 9);
+
+void buttonCallback(ButtonEvent event, int buttonId);
+
+int main()
+{
+    button1.mode(Mode::INPUT);
+    button2.mode(Mode::INPUT);
+
+    ButtonHandler::getInstance().registerButtonCallback(
+        button1, bind(buttonCallback, _1, 1));
+    ButtonHandler::getInstance().registerButtonCallback(
+        button2, bind(buttonCallback, _1, 2));
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+void buttonCallback(ButtonEvent event, int buttonId)
+{
+    switch (event)
+    {
+        case ButtonEvent::PRESSED:
+            printf("Button %d pressed\n", buttonId);
+            break;
+        case ButtonEvent::SHORT_PRESS:
+            printf("Button %d released, it was a short press\n", buttonId);
+            break;
+        case ButtonEvent::LONG_PRESS:
+            printf("Button %d released, it was a long press\n", buttonId);
+            break;
+        case ButtonEvent::VERY_LONG_PRESS:
+            printf("Button %d released, it was a very long press\n", buttonId);
+            break;
+
+        default:
+            break;
+    }
+}
\ No newline at end of file