diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 3bdae31b7848fdeca7f80db8b2cc704f9d842653..24bc7db738a3e58d3eb2b651d9b00cf9dc55724f 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -575,7 +575,7 @@
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "_MIOSIX",
-                "__cplusplus=201103L",
+                "__cplusplus=201402L",
                 "HILSimulation",
                 "ROCCARASO"
             ],
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1a161e91483ff7f6bc0ddfb9e9a81e8b75f6ac63..e25d87503b136e3c61413f26b678f48e6be839f6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,4 +36,18 @@ project(OnBoardSoftware)
 add_executable(test-hil src/entrypoints/HIL/test-hil.cpp ${HIL})
 target_include_directories(test-hil PRIVATE ${OBSW_INCLUDE_DIRS})
 target_compile_definitions(test-hil PRIVATE HILTest)
-sbs_target(test-hil stm32f767zi_compute_unit)
\ No newline at end of file
+sbs_target(test-hil stm32f767zi_compute_unit)
+
+add_executable(main-entry src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
+target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(main-entry stm32f767zi_skyward_death_stack_v4)
+
+add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
+target_include_directories(main-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(main-entry-roccaraso PRIVATE ROCCARASO)
+sbs_target(main-entry-roccaraso stm32f767zi_skyward_death_stack_v4)
+
+add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
+target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(main-entry-euroc PRIVATE EUROC)
+sbs_target(main-entry-euroc stm32f767zi_skyward_death_stack_v4)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 48463b4b699bdcad44db349d6061eb1b607859a7..91c76d1bbb9a664e0b10d376c2a6447333fc80b5 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -29,3 +29,22 @@ set(HIL
     src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
     src/hardware_in_the_loop/HIL/HILTransceiver.cpp
 )
+
+set(MAIN_COMPUTER
+    src/boards/Main/BoardScheduler.cpp
+    src/boards/Main/Sensors/Sensors.cpp
+    src/boards/Main/StateMachines/NASController/NASController.cpp
+    src/boards/Main/Radio/Radio.cpp
+    src/boards/Main/TMRepository/TMRepository.cpp
+    src/boards/Main/CanHandler/CanHandler.cpp
+    src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+    src/boards/Main/Actuators/Actuators.cpp
+    src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
+    src/boards/Main/StateMachines/ADAController/ADAController.cpp
+    src/boards/Main/PinHandler/PinHandler.cpp
+    src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
+    src/boards/Main/StateMachines/ABKController/ABKController.cpp
+    src/boards/Main/StateMachines/MEAController/MEAController.cpp
+    src/boards/Main/StateMachines/Deployment/Deployment.cpp
+    src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
+)
diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..788d526d31865d1f1e65d2a4193504beed6f7f71
--- /dev/null
+++ b/src/boards/Main/Actuators/Actuators.cpp
@@ -0,0 +1,207 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/CanHandler/CanHandler.h>
+#include <Main/Configs/ActuatorsConfig.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace Main::ActuatorsConfig;
+
+namespace Main
+{
+Actuators::Actuators(TaskScheduler* sched) : scheduler(sched)
+{
+    // Create the servo instances
+    servoAbk = new Servo(SERVO_ABK_TIMER, SERVO_ABK_CHANNEL, ABK_MIN_PULSE,
+                         ABK_MAX_PULSE);
+    servoExp = new Servo(SERVO_EXP_TIMER, SERVO_EXP_CHANNEL, EXP_MIN_PULSE,
+                         EXP_MAX_PULSE);
+
+    // Set the buzzer
+    buzzer = new PWM(BUZZER_TIMER, BUZZER_FREQUENCY);
+    buzzer->setDutyCycle(BUZZER_CHANNEL, BUZZER_DUTY_CYCLE);
+
+    // Default disable
+    camOff();
+    cutterOff();
+    // gpios::status_led::low();
+
+    // Init by default the CAN servo positions
+    for (int i = 0; i < ServosList::ServosList_ENUM_END; i++)
+    {
+        CANPositions[i] = 0;
+    }
+}
+
+bool Actuators::start()
+{
+    servoAbk->enable();
+    servoExp->enable();
+
+    // Reset the servo position
+    servoAbk->setPosition(0);
+    servoExp->setPosition(0);
+
+    return scheduler->addTask([&]() { updateBuzzer(); }, BUZZER_UPDATE_PERIOD,
+                              TaskScheduler::Policy::RECOVER);
+}
+
+void Actuators::setServoPosition(ServosList servo, float position)
+{
+    // Lock the mutex for thread sync
+    miosix::Lock<FastMutex> l(mutex);
+    Servo* requestedServo = getServo(servo);
+
+    if (requestedServo != nullptr)
+    {
+        requestedServo->setPosition(position, true);
+    }
+
+    // Log the position
+    Logger::getInstance().log(requestedServo->getState());
+}
+
+void Actuators::setCANServoPosition(ServosList servo, float position)
+{
+    // Lock the mutex for thread sync
+    miosix::Lock<FastMutex> l(mutex);
+
+    CANPositions[servo] = position;
+}
+
+void Actuators::wiggleServo(ServosList servo)
+{
+    Servo* requestedServo = getServo(servo);
+
+    if (requestedServo != nullptr)
+    {
+        // Do not lock the mutex due to set position lock
+        setServoPosition(servo, 1);
+        Thread::sleep(1000);
+        setServoPosition(servo, 0);
+    }
+    else
+    {
+        // Run a CAN message
+        ModuleManager::getInstance().get<CanHandler>()->sendCanCommand(servo, 1,
+                                                                       1000);
+    }
+}
+
+float Actuators::getServoPosition(ServosList servo)
+{
+    miosix::Lock<FastMutex> l(mutex);
+    Servo* requestedServo = getServo(servo);
+
+    if (requestedServo != nullptr)
+    {
+        return requestedServo->getPosition();
+    }
+
+    return CANPositions[servo];
+}
+
+void Actuators::camOn()
+{
+    miosix::Lock<FastMutex> l(mutex);
+    gpios::camera_enable::high();
+}
+
+void Actuators::camOff()
+{
+    miosix::Lock<FastMutex> l(mutex);
+    gpios::camera_enable::low();
+}
+
+void Actuators::cutterOn()
+{
+    miosix::Lock<FastMutex> l(mutex);
+    gpios::cut_trigger::high();
+}
+
+void Actuators::cutterOff()
+{
+    miosix::Lock<FastMutex> l(mutex);
+    gpios::cut_trigger::low();
+}
+
+Servo* Actuators::getServo(ServosList servo)
+{
+    switch (servo)
+    {
+        case AIR_BRAKES_SERVO:
+        {
+            return servoAbk;
+        }
+        case EXPULSION_SERVO:
+        {
+            return servoExp;
+        }
+        default:
+        {
+            return nullptr;
+        }
+    }
+}
+
+void Actuators::setBuzzerArm()
+{
+    // Set the counter with respect to the update function period
+    buzzerCounterOverflow = BUZZER_ARM_PERIOD / BUZZER_UPDATE_PERIOD;
+}
+
+void Actuators::setBuzzerLand()
+{
+    // Set the counter with respect to the update function period
+    buzzerCounterOverflow = BUZZER_LAND_PERIOD / BUZZER_UPDATE_PERIOD;
+}
+
+void Actuators::setBuzzerOff() { buzzerCounterOverflow = 0; }
+
+void Actuators::updateBuzzer()
+{
+    if (buzzerCounterOverflow == 0)
+    {
+        // The buzzer is deactivated thus the channel is disabled
+        buzzer->disableChannel(BUZZER_CHANNEL);
+    }
+    else
+    {
+        if (buzzerCounter >= buzzerCounterOverflow)
+        {
+            // Enable the channel for this period
+            buzzer->enableChannel(BUZZER_CHANNEL);
+            buzzerCounter = 0;
+        }
+        else
+        {
+            buzzer->disableChannel(BUZZER_CHANNEL);
+            buzzerCounter++;
+        }
+    }
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main/Actuators/Actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1e3157cb961400ac6a25194d989441a08af0b3f
--- /dev/null
+++ b/src/boards/Main/Actuators/Actuators.h
@@ -0,0 +1,120 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <actuators/Servo/Servo.h>
+#include <common/Mavlink.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/timer/PWM.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class Actuators : public Boardcore::Module
+{
+public:
+    Actuators(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Enables all the servos
+     */
+    bool start();
+
+    /**
+     * @brief Sets the servo passed servo position in normalized value [0-1]
+     */
+    void setServoPosition(ServosList servo, float position);
+
+    /**
+     * @brief Sets the servo passed servo position inside the CAN structure
+     */
+    void setCANServoPosition(ServosList servo, float position);
+
+    /**
+     * @brief Wiggles the passed servo for 1 second
+     * @note The method is blocking during the second of opening
+     */
+    void wiggleServo(ServosList servo);
+
+    /**
+     * @brief Get the passed servo's position [0-1]
+     */
+    float getServoPosition(ServosList servo);
+
+    // Camera commands
+    void camOn();
+    void camOff();
+
+    // Cutter commands
+    void cutterOn();
+    void cutterOff();
+
+    // Setters for buzzer state
+    void setBuzzerArm();
+    void setBuzzerLand();
+    void setBuzzerOff();
+
+private:
+    /**
+     * @brief Returns the selected servo pointer
+     */
+    Boardcore::Servo* getServo(ServosList servo);
+
+    /**
+     * @brief Automatic called method to update the buzzer status
+     */
+    void updateBuzzer();
+
+    // Connected servos
+    Boardcore::Servo* servoAbk = nullptr;
+    Boardcore::Servo* servoExp = nullptr;
+
+    // Buzzer
+    Boardcore::PWM* buzzer;
+
+    // Status LED state
+    bool ledState = false;
+
+    // Counter that enables and disables the buzzer
+    uint32_t buzzerCounter = 0;
+
+    // Upper limit of the buzzer counter
+    std::atomic<uint32_t> buzzerCounterOverflow{0};
+
+    // Can set servo positions
+    float CANPositions[ServosList::ServosList_ENUM_END];
+
+    // Scheduler for buzzer
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Access to all the actuators mutex, instead of PauseKernel lock to avoid
+    // low priority stuff locking the whole kernel and let the component respect
+    // the priority
+    miosix::FastMutex mutex;
+
+    // Debug logger
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Actuators");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3da65ed747a22aaef039095f86443d4e98e078a3
--- /dev/null
+++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
@@ -0,0 +1,87 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/Configs/AltitudeTriggerConfig.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Common;
+
+namespace Main
+{
+AltitudeTrigger::AltitudeTrigger(TaskScheduler* sched) : scheduler(sched)
+{
+    altitude = AltitudeTriggerConfig::ALTITUDE_REFERENCE;
+}
+
+bool AltitudeTrigger::start()
+{
+    size_t result = scheduler->addTask(
+        [&]() { update(); }, AltitudeTriggerConfig::ALTITUDE_CHECKER_PERIOD,
+        TaskScheduler::Policy::RECOVER);
+
+    return result != 0;
+}
+
+void AltitudeTrigger::setDeploymentAltitude(float altitude)
+{
+    this->altitude = altitude;
+}
+
+void AltitudeTrigger::update()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Check the FMM state
+    FlightModeManagerStatus status =
+        modules.get<FlightModeManager>()->getStatus();
+
+    if (status.state == FlightModeManagerState::DROGUE_DESCENT)
+    {
+        // Get the estimated ADA altitude
+        ADAState state = modules.get<ADAController>()->getADAState();
+
+        // Update the confidence
+        if (state.aglAltitude < altitude)
+        {
+            confidence++;
+        }
+        else
+        {
+            confidence = 0;
+        }
+
+        if (confidence >= AltitudeTriggerConfig::ALTITUDE_CONFIDENCE_THRESHOLD)
+        {
+            // We reached the altitude
+            confidence = 0;
+            EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
+                                            TOPIC_FLIGHT);
+        }
+    }
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
new file mode 100644
index 0000000000000000000000000000000000000000..1eeea13dd25c8849921c35e9c9a1107e522620e7
--- /dev/null
+++ b/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h
@@ -0,0 +1,73 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class AltitudeTrigger : public Boardcore::Module
+{
+public:
+    AltitudeTrigger(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Adds to the task scheduler the periodic update task
+     */
+    bool start();
+
+    /**
+     * @brief Set the Deployment Altitude that must be reached to open the Main
+     * parachute
+     */
+    void setDeploymentAltitude(float alt);
+
+    /**
+     * @brief The update function that is called every period to check the FMM
+     * state and the altitude reached
+     */
+    void update();
+
+private:
+    /**
+     * @brief Reference that must be reached
+     */
+    std::atomic<float> altitude;
+
+    /**
+     * @brief Counts how many times the altitude has been reached consequently.
+     */
+    uint32_t confidence;
+
+    /**
+     * @brief Scheduler to which add the periodic update task
+     */
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("AltitudeTrigger");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/BoardScheduler.cpp b/src/boards/Main/BoardScheduler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b380b32df78b25b55ba117af04f24126a79af6b7
--- /dev/null
+++ b/src/boards/Main/BoardScheduler.cpp
@@ -0,0 +1,65 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "BoardScheduler.h"
+
+using namespace Boardcore;
+
+namespace Main
+{
+BoardScheduler::BoardScheduler()
+{
+    scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
+    scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
+    scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
+    scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
+}
+
+TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
+{
+    switch (priority.get())
+    {
+        case miosix::PRIORITY_MAX:
+            return scheduler4;
+        case miosix::PRIORITY_MAX - 1:
+            return scheduler3;
+        case miosix::PRIORITY_MAX - 2:
+            return scheduler2;
+        case miosix::MAIN_PRIORITY:
+            return scheduler1;
+        default:
+            return scheduler1;
+    }
+}
+
+bool BoardScheduler::start()
+{
+    return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
+           scheduler4->start();
+}
+
+bool BoardScheduler::isStarted()
+{
+    return scheduler1->isRunning() && scheduler2->isRunning() &&
+           scheduler3->isRunning() && scheduler4->isRunning();
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/BoardScheduler.h b/src/boards/Main/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b39c023c8a59a37ff42744587dfbc09e01f2812
--- /dev/null
+++ b/src/boards/Main/BoardScheduler.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+/**
+ * @brief Class that wraps the 4 main task schedulers of the entire OBSW.
+ * There is a task scheduler for every miosix priority
+ */
+class BoardScheduler : public Boardcore::Module
+{
+public:
+    BoardScheduler();
+
+    /**
+     * @brief Get the Scheduler object relative to the requested priority
+     *
+     * @param priority The task scheduler priority
+     * @return Boardcore::TaskScheduler& Reference to the requested task
+     * scheduler.
+     * @note Min priority scheduler is returned in case of non valid priority.
+     */
+    Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Returns if all the schedulers are up and running
+     */
+    bool isStarted();
+
+private:
+    Boardcore::TaskScheduler* scheduler1;
+    Boardcore::TaskScheduler* scheduler2;
+    Boardcore::TaskScheduler* scheduler3;
+    Boardcore::TaskScheduler* scheduler4;
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..13552ec6be401c1d3be9ebd4456e4da4e94d3b9a
--- /dev/null
+++ b/src/boards/Main/Buses.h
@@ -0,0 +1,55 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/i2c/I2C.h>
+#include <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+namespace Main
+{
+class Buses : public Boardcore::Module
+{
+public:
+    Boardcore::SPIBus spi1;
+    Boardcore::SPIBus spi3;
+    Boardcore::SPIBus spi4;
+    Boardcore::SPIBus spi6;
+
+    Boardcore::I2C i2c1;
+
+    Boardcore::USART usart1;
+    Boardcore::USART usart2;
+    Boardcore::USART uart4;
+
+    Buses()
+        : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6),
+          i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(),
+               miosix::interfaces::i2c1::sda::getPin()),
+          usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200)
+    {
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main/CanHandler/CanHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc172db9c0976f766d3a359a5d9f0c429629979a
--- /dev/null
+++ b/src/boards/Main/CanHandler/CanHandler.cpp
@@ -0,0 +1,294 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "CanHandler.h"
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Buses.h>
+#include <Main/CanHandler/CanHandlerData.h>
+#include <Main/Configs/CanHandlerConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/CanConfig.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+
+#include <functional>
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace Common;
+using namespace CanConfig;
+using namespace Main::CanHandlerConfig;
+
+namespace Main
+{
+CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+{
+    // Configure the CAN driver
+    CanbusDriver::AutoBitTiming bitTiming;
+    bitTiming.baudRate    = BAUD_RATE;
+    bitTiming.samplePoint = SAMPLE_POINT;
+
+    CanbusDriver::CanbusConfig config;
+
+    // Configure the correct peripheral
+    driver = new CanbusDriver(CAN2, config, bitTiming);
+
+    // Create the protocol with the defined driver
+    protocol =
+        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
+                        miosix::PRIORITY_MAX - 1);
+
+    // Accept messages only from the main and RIG board
+    protocol->addFilter(static_cast<uint8_t>(Board::PAYLOAD),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::RIG),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::MOTOR),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    driver->init();
+}
+
+bool CanHandler::start()
+{
+    // 0 if fail
+    uint8_t result = scheduler->addTask(  // status
+        [&]()
+        {
+            FlightModeManagerState state = ModuleManager::getInstance()
+                                               .get<FlightModeManager>()
+                                               ->getStatus()
+                                               .state;
+            protocol->enqueueSimplePacket(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::STATUS),
+                static_cast<uint8_t>(Board::MAIN),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(state),
+                ((state == FlightModeManagerState::ARMED) ? 0x01 : 0x00));
+        },
+        STATUS_TRANSMISSION_PERIOD);
+
+    return protocol->start() && result != 0;
+}
+
+bool CanHandler::isStarted()
+{
+    return protocol->isStarted() && scheduler->isRunning();
+}
+
+void CanHandler::sendEvent(EventId event)
+{
+    protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL),
+                           static_cast<uint8_t>(PrimaryType::EVENTS),
+                           static_cast<uint8_t>(Board::MAIN),
+                           static_cast<uint8_t>(Board::BROADCAST),
+                           static_cast<uint8_t>(event));
+}
+
+void CanHandler::sendCanCommand(ServosList servo, bool targetState,
+                                uint32_t delay)
+{
+    uint64_t payload = delay;
+    payload          = payload << 8;
+    payload          = payload | targetState;
+    protocol->enqueueSimplePacket(static_cast<uint8_t>(Priority::CRITICAL),
+                                  static_cast<uint8_t>(PrimaryType::COMMAND),
+                                  static_cast<uint8_t>(Board::MAIN),
+                                  static_cast<uint8_t>(Board::BROADCAST),
+                                  static_cast<uint8_t>(servo), payload);
+}
+
+void CanHandler::handleCanMessage(const CanMessage &msg)
+{
+    PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType());
+
+    // Depending on the received message, call the handling method
+    switch (msgType)
+    {
+        case PrimaryType::EVENTS:
+        {
+            handleCanEvent(msg);
+            break;
+        }
+        case PrimaryType::SENSORS:
+        {
+            handleCanSensor(msg);
+            break;
+        }
+        case PrimaryType::ACTUATORS:
+        {
+            handleCanActuator(msg);
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported message type: type={}",
+                     msgType);
+            break;
+        }
+    }
+}
+
+void CanHandler::handleCanEvent(const CanMessage &msg)
+{
+    EventId eventId = static_cast<EventId>(msg.getSecondaryType());
+    auto it         = eventToEvent.find(eventId);
+
+    // Check if the event is valid
+    if (it != eventToEvent.end())
+    {
+        EventBroker::getInstance().post(it->second, TOPIC_CAN);
+    }
+    else
+    {
+        LOG_WARN(logger, "Received unsupported event: id={}", eventId);
+    }
+}
+
+void CanHandler::handleCanSensor(const CanMessage &msg)
+{
+    SensorId sensorId      = static_cast<SensorId>(msg.getSecondaryType());
+    ModuleManager &modules = ModuleManager::getInstance();
+
+    // Depending on the sensor call the corresponding setter on the Sensors
+    // module
+    switch (sensorId)
+    {
+        case SensorId::PITOT:
+        {
+            PitotData data = pitotDataFromCanMessage(msg);
+            modules.get<Sensors>()->setPitot(data);
+
+            // Log the data
+            data.timestamp = TimestampTimer::getTimestamp();
+            Logger::getInstance().log(data);
+
+            break;
+        }
+        case SensorId::CC_PRESSURE:
+        {
+            PressureData data = pressureDataFromCanMessage(msg);
+            modules.get<Sensors>()->setCCPressure(data);
+
+            // Log the data
+            CanPressureSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.pressure  = data.pressure;
+            log.sensorId  = static_cast<uint8_t>(SensorId::CC_PRESSURE);
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::BOTTOM_TANK_PRESSURE:
+        {
+            PressureData data = pressureDataFromCanMessage(msg);
+            modules.get<Sensors>()->setBottomTankPressure(data);
+
+            // Log the data
+            CanPressureSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.pressure  = data.pressure;
+            log.sensorId = static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE);
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::TOP_TANK_PRESSURE:
+        {
+            PressureData data = pressureDataFromCanMessage(msg);
+            modules.get<Sensors>()->setTopTankPressure(data);
+
+            // Log the data
+            CanPressureSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.pressure  = data.pressure;
+            log.sensorId  = static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE);
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::TANK_TEMPERATURE:
+        {
+            TemperatureData data = temperatureDataFromCanMessage(msg);
+            modules.get<Sensors>()->setTankTemperature(data);
+
+            // Log the data
+            CanTemperatureSensor log;
+            log.timestamp   = TimestampTimer::getTimestamp();
+            log.temperature = data.temperature;
+            log.sensorId    = static_cast<uint8_t>(SensorId::TANK_TEMPERATURE);
+
+            break;
+        }
+        case SensorId::MOTOR_BOARD_VOLTAGE:
+        {
+            BatteryVoltageSensorData data = voltageDataFromCanMessage(msg);
+            modules.get<Sensors>()->setMotorBatteryVoltage(data);
+
+            // Log the data
+            CanVoltageSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.sensorId  = static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE);
+            log.voltage   = data.batVoltage;
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::MOTOR_ACTUATORS_CURRENT:
+        {
+            CurrentData data = currentDataFromCanMessage(msg);
+            modules.get<Sensors>()->setMotorCurrent(data);
+
+            // Log the data
+            CanCurrentSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.sensorId =
+                static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT);
+            log.current = data.current;
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported sensor data: id={}",
+                     sensorId);
+        }
+    }
+}
+
+void CanHandler::handleCanActuator(const CanMessage &msg)
+{
+    ServosList servo       = static_cast<ServosList>(msg.getSecondaryType());
+    ModuleManager &modules = ModuleManager::getInstance();
+
+    // Set the servo position
+    modules.get<Actuators>()->setCANServoPosition(
+        servo, servoDataFromCanMessage(msg).position);
+}
+
+}  // namespace Main
diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main/CanHandler/CanHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e585f00c7c559c2b60e7807a0f325c0dcba188f
--- /dev/null
+++ b/src/boards/Main/CanHandler/CanHandler.h
@@ -0,0 +1,85 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Federico Mandelli, Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/BoardScheduler.h>
+#include <common/CanConfig.h>
+#include <common/Mavlink.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+
+class CanHandler : public Boardcore::Module
+{
+
+public:
+    explicit CanHandler(Boardcore::TaskScheduler *sched);
+
+    /**
+     * @brief Adds the periodic task to the scheduler and starts the protocol
+     * threads
+     */
+    bool start();
+
+    /**
+     * @brief Returns true if the protocol threads are started and the scheduler
+     * is running
+     */
+    bool isStarted();
+
+    /**
+     * @brief Sends a CAN event on the bus
+     */
+    void sendEvent(Common::CanConfig::EventId event);
+
+    /**
+     * @brief Sends a can command (servo actuation command) specifying the
+     * target servo, the target state and eventually the delta [ms] in which the
+     * servo remains open
+     */
+    void sendCanCommand(ServosList servo, bool targetState, uint32_t delay);
+
+private:
+    /**
+     * @brief Handles a generic CAN message and dispatch the message to the
+     * correct handler
+     */
+    void handleCanMessage(const Boardcore::Canbus::CanMessage &msg);
+
+    // CAN message handlers
+    void handleCanEvent(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanSensor(const Boardcore::Canbus::CanMessage &msg);
+    void handleCanActuator(const Boardcore::Canbus::CanMessage &msg);
+
+    // CAN interfaces
+    Boardcore::Canbus::CanbusDriver *driver;
+    Boardcore::Canbus::CanProtocol *protocol;
+
+    Boardcore::TaskScheduler *scheduler;
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler");
+};
+
+}  // namespace Main
diff --git a/src/boards/Main/CanHandler/CanHandlerData.h b/src/boards/Main/CanHandler/CanHandlerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..e25d62bc71b907a6cb8f567f5ca5d6a4d8f04b6d
--- /dev/null
+++ b/src/boards/Main/CanHandler/CanHandlerData.h
@@ -0,0 +1,92 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+// This class defines all the types received from the CAN bus
+struct CanPressureSensor
+{
+    uint64_t timestamp = 0;
+    uint8_t sensorId   = 0;
+    float pressure     = 0;
+
+    static std::string header() { return "timestamp,sensorId,pressure"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)sensorId << "," << pressure << "\n";
+    }
+};
+
+struct CanTemperatureSensor
+{
+    uint64_t timestamp = 0;
+    uint8_t sensorId   = 0;
+    float temperature  = 0;
+
+    static std::string header() { return "timestamp,sensorId,temperature"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)sensorId << "," << temperature << "\n";
+    }
+};
+
+struct CanCurrentSensor
+{
+    uint64_t timestamp = 0;
+    uint8_t sensorId   = 0;
+    uint8_t boardId    = 0;
+    float current      = 0;
+
+    static std::string header() { return "timestamp,sensorId,boardId,current"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)sensorId << "," << (int)boardId << ","
+           << current << "\n";
+    }
+};
+
+struct CanVoltageSensor
+{
+    uint64_t timestamp = 0;
+    uint8_t sensorId   = 0;
+    uint8_t boardId    = 0;
+    float voltage      = 0;
+
+    static std::string header() { return "timestamp,sensorId,boardId,voltage"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)sensorId << "," << (int)boardId << ","
+           << voltage << "\n";
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/ABKConfig.h b/src/boards/Main/Configs/ABKConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e8aadec95bbfd5c3af985f68ef8dec4c564a6ac
--- /dev/null
+++ b/src/boards/Main/Configs/ABKConfig.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+#include <algorithms/AirBrakes/AirBrakesInterp.h>
+
+namespace Main
+{
+namespace ABKConfig
+{
+// TODO remove this useless config from interpolation algorithm
+static const Boardcore::AirBrakesConfig ABK_CONFIG{
+    0.4884,      -1.4391,    6.6940,
+    -18.4272,    29.1044,    -24.5585,
+    8.6058,      9.0426,     159.5995,
+    4.8188,      -208.4471,  47.0771,
+    1.9433e+03,  -205.6689,  -6.4634e+03,
+    331.0332,    8.8763e+03, -161.8111,
+    -3.9917e+03, 2.8025e-06, 0.0373,
+    20,          -0.009216,  0.02492,
+    -0.01627,    0.03191,    0.017671458676443,
+    0,
+};
+
+constexpr uint32_t UPDATE_PERIOD = 100;  // [ms] -> 10Hz
+
+// ABK algorithm configs
+constexpr float MINIMUM_ALTITUDE      = 400;
+constexpr float MAXIMUM_ALTITUDE      = 1000;
+constexpr float STARTING_FILTER_VALUE = 0.75f;
+constexpr float ABK_CRITICAL_ALTITUDE = 970;
+constexpr float DZ                    = 10;
+constexpr float INITIAL_MASS          = 26;
+constexpr float DM                    = 0.1f;
+constexpr uint16_t N_FORWARD          = 0;
+
+// Shadow mode after motor shutdown to let MEA algorithm correctly estimate the
+// mass
+constexpr uint32_t DELAY_TIMEOUT = 500;  // [ms] -> 0.5s
+}  // namespace ABKConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..41be4c220825e2acc1ffd4fe8bcd2c80f9f5b24e
--- /dev/null
+++ b/src/boards/Main/Configs/ADAConfig.h
@@ -0,0 +1,50 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Main
+{
+namespace ADAConfig
+{
+constexpr uint32_t UPDATE_PERIOD = 20;                       // [ms] 50Hz
+constexpr float SAMPLING_PERIOD  = UPDATE_PERIOD / 1000.0f;  // [seconds]
+
+// Calibration constants
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr int CALIBRATION_SLEEP_TIME    = 100;  // [ms]
+
+// ADA shadow mode time, during which the ADA algorithm cannot trigger apogees
+constexpr uint32_t SHADOW_MODE_TIMEOUT = 10000;  // [ms]
+
+// When the vertical speed is smaller than this value, apogee is detected.
+// If equal to 0 ->     Exact apogee
+// If greater than 0 -> Apogee detected ahead of time (while still going up)
+constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5;  // [m/s]
+
+// Number of consecutive samples after which apogee is triggered.
+constexpr unsigned int APOGEE_N_SAMPLES = 5;
+
+}  // namespace ADAConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/ActuatorsConfig.h b/src/boards/Main/Configs/ActuatorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..18617d06cdbb17028e361c6131f87e0ea0583de6
--- /dev/null
+++ b/src/boards/Main/Configs/ActuatorsConfig.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+#include <drivers/timer/PWM.h>
+#include <drivers/timer/TimerUtils.h>
+
+namespace Main
+{
+namespace ActuatorsConfig
+{
+static TIM_TypeDef* const SERVO_ABK_TIMER = TIM3;
+static TIM_TypeDef* const SERVO_EXP_TIMER = TIM12;
+static TIM_TypeDef* const BUZZER_TIMER    = TIM1;
+
+// TODO change correspondent naming in hwmapping (channel different)
+constexpr Boardcore::TimerUtils::Channel SERVO_ABK_CHANNEL =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+constexpr Boardcore::TimerUtils::Channel SERVO_EXP_CHANNEL =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+
+constexpr uint16_t ABK_MIN_PULSE = 1950;
+constexpr uint16_t ABK_MAX_PULSE = 1380;
+
+// Inverted to invert the servo logic
+constexpr uint16_t EXP_MIN_PULSE = 900;
+constexpr uint16_t EXP_MAX_PULSE = 2000;
+
+// Buzzer configs
+constexpr uint32_t BUZZER_FREQUENCY = 1000;
+constexpr float BUZZER_DUTY_CYCLE   = 0.5;
+
+constexpr uint32_t BUZZER_UPDATE_PERIOD = 100;  // [ms]
+
+constexpr uint32_t BUZZER_ARM_PERIOD  = 500;   // [ms]
+constexpr uint32_t BUZZER_LAND_PERIOD = 1000;  // [ms]
+
+}  // namespace ActuatorsConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/AltitudeTriggerConfig.h b/src/boards/Main/Configs/AltitudeTriggerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3ec7f2a8d14204ee05b4aa03933e74f46c15f61
--- /dev/null
+++ b/src/boards/Main/Configs/AltitudeTriggerConfig.h
@@ -0,0 +1,35 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Main
+{
+namespace AltitudeTriggerConfig
+{
+constexpr uint32_t ALTITUDE_CHECKER_PERIOD       = 100;  //[ms] -> 10Hz
+constexpr uint32_t ALTITUDE_REFERENCE            = 350;  //[m]
+constexpr uint32_t ALTITUDE_CONFIDENCE_THRESHOLD = 5;
+}  // namespace AltitudeTriggerConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main/Configs/CanHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc9ff47896cbd7c21348f25d156aa52e1facebb6
--- /dev/null
+++ b/src/boards/Main/Configs/CanHandlerConfig.h
@@ -0,0 +1,38 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <common/Events.h>
+
+namespace Main
+{
+
+namespace CanHandlerConfig
+{
+
+constexpr unsigned int PITOT_TRANSMISSION_PERIOD  = 20;    // ms
+constexpr unsigned int STATUS_TRANSMISSION_PERIOD = 2000;  // ms
+
+}  // namespace CanHandlerConfig
+
+}  // namespace Main
diff --git a/src/boards/Main/Configs/DeploymentConfig.h b/src/boards/Main/Configs/DeploymentConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..83a237255382fdb1d78d1ef2c55031d68acd15dc
--- /dev/null
+++ b/src/boards/Main/Configs/DeploymentConfig.h
@@ -0,0 +1,31 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+namespace Main
+{
+namespace DeploymentConfig
+{
+constexpr unsigned int CUT_DURATION = 500;  // [ms]
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main/Configs/FlightModeManagerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..de12450f4b2e4062565d1e3d26f815f91986519c
--- /dev/null
+++ b/src/boards/Main/Configs/FlightModeManagerConfig.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Angelo Prete
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+namespace Main
+{
+namespace FMMConfig
+{
+constexpr unsigned int MISSION_TIMEOUT         = 15 * 60 * 1000;
+constexpr unsigned int ENGINE_SHUTDOWN_TIMEOUT = 4000;   // [ms]
+constexpr unsigned int APOGEE_EVENT_TIMEOUT    = 40000;  // [ms]
+}  // namespace FMMConfig
+
+}  // namespace Main
diff --git a/src/boards/Main/Configs/FlightStatsRecorderConfig.h b/src/boards/Main/Configs/FlightStatsRecorderConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..45a80b3ae65ba435bc0ee63848316d7726597dc1
--- /dev/null
+++ b/src/boards/Main/Configs/FlightStatsRecorderConfig.h
@@ -0,0 +1,33 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Main
+{
+namespace FlightStatsRecorderConfig
+{
+constexpr uint32_t FSR_UPDATE_PERIOD = 100;  // [ms]
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/MEAConfig.h b/src/boards/Main/Configs/MEAConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed242b76f702baaa9ec8de5bd7ebde27aba41077
--- /dev/null
+++ b/src/boards/Main/Configs/MEAConfig.h
@@ -0,0 +1,54 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Main
+{
+namespace MEAConfig
+{
+constexpr uint32_t UPDATE_PERIOD = 20;  // [ms] -> 50Hz
+
+constexpr float SENSOR_NOISE_VARIANCE       = 0.36f;
+constexpr float MODEL_NOISE_VARIANCE        = 0.1f;
+constexpr float DEFAULT_INITIAL_ROCKET_MASS = 28.9286227299384f;
+
+constexpr uint32_t SHADOW_MODE_TIMEOUT      = 900000;  // [ms]
+constexpr float SHUTDOWN_THRESHOLD_ALTITUDE = 1000;    // [m]
+constexpr unsigned int SHUTDOWN_N_SAMPLES   = 5;
+
+// Pressure threshold after which the kalman is updated
+constexpr float CC_PRESSURE_THRESHOLD = 1.f;
+
+// Aerodynamics coefficients
+constexpr float n000 = 0.554395329698886;
+constexpr float n100 = -1.71019994711676;
+constexpr float n200 = 8.05103009321528;
+constexpr float n300 = -22.2129400612042;
+constexpr float n400 = 34.6990670331566;
+constexpr float n500 = -28.6219169089691;
+constexpr float n600 = 9.73349655723146;
+
+}  // namespace MEAConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..486c4e8d3d577f78b7824b1169d902f78cbefbc2
--- /dev/null
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -0,0 +1,68 @@
+/* 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 <algorithms/NAS/NASConfig.h>
+#include <algorithms/ReferenceValues.h>
+#include <common/ReferenceConfig.h>
+
+namespace Main
+{
+
+namespace NASConfig
+{
+
+constexpr uint32_t UPDATE_PERIOD = 20;  // 50 hz
+
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr int CALIBRATION_SLEEP_TIME    = 100;  // [ms]
+
+constexpr float ACCELERATION_THRESHOLD = 0.5;  // [m/s^2]
+
+// Threshold to re-enable the accelerometer readings
+constexpr int ACCELERATION_THRESHOLD_SAMPLE = 20;
+
+static const Boardcore::NASConfig config = {
+    UPDATE_PERIOD / 1000.0,          // T
+    0.0001f,                         // SIGMA_BETA
+    0.3f,                            // SIGMA_W
+    0.1f,                            // SIGMA_ACC
+    0.1f,                            // SIGMA_MAG
+    10.0f,                           // SIGMA_GPS
+    4.3f,                            // SIGMA_BAR
+    10.0f,                           // SIGMA_POS
+    10.0f,                           // SIGMA_VEL
+    10.0f,                           // SIGMA_PITOT
+    1.0f,                            // P_POS
+    10.0f,                           // P_POS_VERTICAL
+    1.0f,                            // P_VEL
+    10.0f,                           // P_VEL_VERTICAL
+    0.01f,                           // P_ATT
+    0.01f,                           // P_BIAS
+    6.0f,                            // SATS_NUM
+    Common::ReferenceConfig::nedMag  // NED_MAG
+};
+
+}  // namespace NASConfig
+
+}  // namespace Main
diff --git a/src/boards/Main/Configs/PinHandlerConfig.h b/src/boards/Main/Configs/PinHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8b3f38274a7c65f2632d44dee9b940295803d3e
--- /dev/null
+++ b/src/boards/Main/Configs/PinHandlerConfig.h
@@ -0,0 +1,42 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <utils/PinObserver/PinObserver.h>
+
+namespace Main
+{
+namespace PinHandlerConfig
+{
+constexpr unsigned int LAUNCH_PIN_THRESHOLD = 20;
+constexpr Boardcore::PinTransition LAUNCH_PIN_TRIGGER =
+    Boardcore::PinTransition::RISING_EDGE;
+
+constexpr unsigned int NC_DETACH_PIN_THRESHOLD = 20;
+constexpr Boardcore::PinTransition NC_DETACH_PIN_TRIGGER =
+    Boardcore::PinTransition::RISING_EDGE;
+
+constexpr unsigned int CUTTER_SENSE_PIN_THRESHOLD = 20;
+constexpr unsigned int EXPULSION_PIN_THRESHOLD    = 20;
+}  // namespace PinHandlerConfig
+}  // namespace Main
diff --git a/src/boards/Main/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..42354f01172a5f62b245cef278681457b5e1681f
--- /dev/null
+++ b/src/boards/Main/Configs/RadioConfig.h
@@ -0,0 +1,46 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <common/Mavlink.h>
+
+namespace Main
+{
+namespace RadioConfig
+{
+// Mavlink driver template parameters
+constexpr uint32_t RADIO_PKT_LENGTH     = 255;
+constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20;
+constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+
+constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250;
+
+constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50;
+constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10;
+
+constexpr uint8_t MAV_SYSTEM_ID = 171;
+constexpr uint8_t MAV_COMP_ID   = 96;
+
+constexpr uint32_t MAVLINK_QUEUE_SIZE = 10;
+
+}  // namespace RadioConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..e6d686fe21cebaaac4a59f9655712fd82bbd1d02
--- /dev/null
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -0,0 +1,121 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+
+namespace Main
+{
+namespace SensorsConfig
+{
+constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4;
+constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100;
+
+constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4;
+constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100;
+constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR =
+    Boardcore::LPS28DFW::FS_1260;
+
+constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR =
+    Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400;
+constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU =
+    Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE;
+constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR =
+    Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100;
+
+constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE =
+    Boardcore::LIS2MDL::MD_CONTINUOUS;
+constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ;
+constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5;
+
+constexpr uint8_t UBXGPS_SAMPLE_RATE = 5;
+
+constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU =
+    Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+
+constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS =
+    Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16;
+constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR =
+    Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416;
+constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE =
+    Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+
+constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS =
+    Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000;
+constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR =
+    Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416;
+
+constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE =
+    Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION
+    LSM6DSRX_FIFO_TIMESTAMP_DECIMATION =
+        Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
+constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR
+    LSM6DSRX_FIFO_TEMPERATURE_BDR =
+        Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
+
+constexpr Boardcore::ADS131M08Defs::OversamplingRatio
+    ADS131M08_OVERSAMPLING_RATIO =
+        Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
+constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true;
+
+constexpr uint32_t LPS22DF_PERIOD         = 20;  // [ms] 50Hz
+constexpr uint32_t LPS28DFW_PERIOD        = 20;  // [ms] 50Hz
+constexpr uint32_t H3LIS331DL_PERIOD      = 10;  // [ms] 100Hz
+constexpr uint32_t LIS2MDL_PERIOD         = 10;  // [ms] 100Hz
+constexpr uint32_t UBXGPS_PERIOD          = 1000 / UBXGPS_SAMPLE_RATE;  // [ms]
+constexpr uint32_t LSM6DSRX_PERIOD        = 20;  // [ms] 50Hz
+constexpr uint32_t ADS131M08_PERIOD       = 10;  // [ms] 100Hz
+constexpr uint32_t IMU_PERIOD             = 20;  // [ms] Fake processed IMU 50Hz
+constexpr uint32_t MAG_CALIBRATION_PERIOD = 100;  // [ms] 10Hz
+
+// ADC sensors configs
+constexpr float ADC_VOLTAGE_RANGE = 1.2f;  // [V] Voltage reading range
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_3;
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_4;
+constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 =
+    Boardcore::ADS131M08Defs::Channel::CHANNEL_2;
+
+// ADC Voltage divider
+constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR =
+    (20.f / 2.4f) +
+    1;  // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider
+constexpr float CURRENT_CONVERSION_FACTOR =
+    (20.f / 4.f) / (12.f / (12.f + 33.f));
+constexpr float CURRENT_OFFSET = 0.133333333f;  // V in ADC
+
+// Calibration samples
+constexpr unsigned int CALIBRATION_SAMPLES = 20;
+constexpr unsigned int CALIBRATION_PERIOD  = 100;
+
+constexpr unsigned int NUMBER_OF_SENSORS = 8;
+
+}  // namespace SensorsConfig
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95766cffc0a0099484772e9d4d680e57a342b560
--- /dev/null
+++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -0,0 +1,222 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Configs/FlightStatsRecorderConfig.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+
+#include <Eigen/Core>
+
+using namespace Boardcore;
+using namespace Main::FlightStatsRecorderConfig;
+using namespace Eigen;
+using namespace miosix;
+
+namespace Main
+{
+FlightStatsRecorder::FlightStatsRecorder(TaskScheduler* sched)
+    : scheduler(sched)
+{
+    // Init the data structure to avoid UB
+    stats.ada_min_pressure      = 0;
+    stats.apogee_alt            = 0;
+    stats.apogee_lat            = 0;
+    stats.apogee_lon            = 0;
+    stats.apogee_ts             = 0;
+    stats.cpu_load              = 0;
+    stats.dpl_max_acc           = 0;
+    stats.dpl_ts                = 0;
+    stats.dpl_vane_max_pressure = 0;
+    stats.free_heap             = 0;
+    stats.liftoff_max_acc       = 0;
+    stats.liftoff_max_acc_ts    = 0;
+    stats.liftoff_ts            = 0;
+    stats.max_airspeed_pitot    = 0;
+    stats.max_speed_altitude    = 0;
+    stats.max_z_speed           = 0;
+    stats.max_z_speed_ts        = 0;
+    stats.min_pressure          = 0;
+}
+
+bool FlightStatsRecorder::start()
+{
+    return scheduler->addTask([&]() { update(); }, FSR_UPDATE_PERIOD,
+                              TaskScheduler::Policy::RECOVER);
+}
+
+void FlightStatsRecorder::update()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // FMM state
+    FlightModeManagerState flightState =
+        modules.get<FlightModeManager>()->getStatus().state;
+
+    // Data gathering
+    AccelerometerData accData = modules.get<Sensors>()->getIMULastSample();
+    PressureData baroData =
+        modules.get<Sensors>()->getStaticPressure1LastSample();
+    PressureData DPLPressure =
+        modules.get<Sensors>()->getDeploymentPressureLastSample();
+    PitotData pitotData = modules.get<Sensors>()->getPitotLastSample();
+    GPSData gpsData     = modules.get<Sensors>()->getGPSLastSample();
+    NASState nasData    = modules.get<NASController>()->getNasState();
+    ADAState adaData    = modules.get<ADAController>()->getADAState();
+
+    // Store the apogee ts
+    uint64_t previousApogee = stats.apogee_ts;
+
+    // Update the data
+    updateAcc(flightState, accData);
+    updateBaro(flightState, baroData);
+    updateDPLPressure(flightState, DPLPressure);
+    updatePitot(flightState, pitotData);
+    updateNAS(flightState, nasData);
+    updateADA(flightState, adaData);
+
+    // If the apogee ts is different update the GPS apogee coordinates
+    if (previousApogee != stats.apogee_ts)
+    {
+        Lock<FastMutex> l(mutex);
+
+        stats.apogee_lat = gpsData.latitude;
+        stats.apogee_lon = gpsData.longitude;
+    }
+}
+
+mavlink_rocket_stats_tm_t FlightStatsRecorder::getStats()
+{
+    Lock<FastMutex> l(mutex);
+    return stats;
+}
+
+void FlightStatsRecorder::updateAcc(FlightModeManagerState flightState,
+                                    AccelerometerData data)
+{
+    Vector3f accelerations = static_cast<Vector3f>(data);
+    float norm             = accelerations.norm();
+
+    if (flightState == FlightModeManagerState::POWERED_ASCENT ||
+        flightState == FlightModeManagerState::UNPOWERED_ASCENT)
+    {
+        Lock<FastMutex> l(mutex);
+
+        stats.liftoff_max_acc = std::max(stats.liftoff_max_acc, norm);
+        bool changed          = stats.liftoff_max_acc == norm;
+
+        // In case of a change update the timestamp
+        stats.liftoff_max_acc_ts =
+            changed ? data.accelerationTimestamp : stats.liftoff_max_acc_ts;
+    }
+    else if (flightState == FlightModeManagerState::DROGUE_DESCENT)
+    {
+        Lock<FastMutex> l(mutex);
+
+        stats.dpl_max_acc = std::max(stats.dpl_max_acc, norm);
+
+        // Change the timestamp of DPL if not already done
+        stats.dpl_ts =
+            stats.dpl_ts == 0 ? data.accelerationTimestamp : stats.dpl_ts;
+    }
+}
+
+void FlightStatsRecorder::updateBaro(FlightModeManagerState flightState,
+                                     PressureData data)
+{
+    Lock<FastMutex> l(mutex);
+
+    if (stats.min_pressure == 0)
+    {
+        stats.min_pressure = data.pressure;
+    }
+
+    // Update the min
+    stats.min_pressure = std::min(stats.min_pressure, data.pressure);
+}
+
+void FlightStatsRecorder::updateDPLPressure(FlightModeManagerState flightState,
+                                            PressureData data)
+{
+    if (flightState == FlightModeManagerState::UNPOWERED_ASCENT ||
+        flightState == FlightModeManagerState::DROGUE_DESCENT)
+    {
+        Lock<FastMutex> l(mutex);
+
+        stats.dpl_vane_max_pressure =
+            std::max(stats.dpl_vane_max_pressure, data.pressure);
+    }
+}
+
+void FlightStatsRecorder::updatePitot(FlightModeManagerState flightState,
+                                      PitotData data)
+{
+    Lock<FastMutex> l(mutex);
+
+    stats.max_airspeed_pitot =
+        std::max(stats.max_airspeed_pitot, data.airspeed);
+}
+
+void FlightStatsRecorder::updateNAS(FlightModeManagerState flightState,
+                                    NASState state)
+{
+    if (flightState == FlightModeManagerState::POWERED_ASCENT ||
+        flightState == FlightModeManagerState::UNPOWERED_ASCENT)
+    {
+        Lock<FastMutex> l(mutex);
+
+        stats.max_z_speed = std::max(stats.max_z_speed, -state.vd);
+        bool changed      = stats.max_z_speed == -state.vd;
+
+        // Change the max speed altitude and timestamp if the max speed changed
+        stats.max_speed_altitude =
+            changed ? -state.d : stats.max_speed_altitude;
+
+        stats.max_z_speed_ts = changed ? state.timestamp : stats.max_z_speed_ts;
+    }
+}
+
+void FlightStatsRecorder::updateADA(FlightModeManagerState flightState,
+                                    ADAState state)
+{
+    if (flightState == FlightModeManagerState::POWERED_ASCENT ||
+        flightState == FlightModeManagerState::UNPOWERED_ASCENT ||
+        flightState == FlightModeManagerState::DROGUE_DESCENT)
+    {
+        Lock<FastMutex> l(mutex);
+
+        if (stats.ada_min_pressure == 0)
+        {
+            stats.ada_min_pressure = state.x0;
+        }
+
+        stats.ada_min_pressure = std::min(stats.ada_min_pressure, state.x0);
+        bool changed           = stats.ada_min_pressure == state.x0;
+
+        stats.apogee_alt = std::max(stats.apogee_alt, state.aglAltitude);
+        stats.apogee_ts  = changed ? state.timestamp : stats.apogee_ts;
+    }
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..59e4fe5c7edc4cadf754848abcd69579c9af145a
--- /dev/null
+++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h
@@ -0,0 +1,82 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <algorithms/ADA/ADAData.h>
+#include <algorithms/NAS/NASState.h>
+#include <common/Mavlink.h>
+#include <scheduler/TaskScheduler.h>
+#include <sensors/SensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class FlightStatsRecorder : public Boardcore::Module
+{
+public:
+    FlightStatsRecorder(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Adds a task to the scheduler to update the stats
+     */
+    bool start();
+
+    /**
+     * @brief Update method that gathers all the info to update the data
+     * structure
+     */
+    void update();
+
+    /**
+     * @brief Gets the packet already populated
+     */
+    mavlink_rocket_stats_tm_t getStats();
+
+private:
+    // Update methods
+    void updateAcc(FlightModeManagerState flightState,
+                   Boardcore::AccelerometerData data);
+    void updateBaro(FlightModeManagerState flightState,
+                    Boardcore::PressureData data);
+    void updateDPLPressure(FlightModeManagerState flightState,
+                           Boardcore::PressureData data);
+    void updatePitot(FlightModeManagerState flightState,
+                     Boardcore::PitotData data);
+    void updateNAS(FlightModeManagerState flightState,
+                   Boardcore::NASState data);
+    void updateADA(FlightModeManagerState flightState,
+                   Boardcore::ADAState data);
+
+    // Update scheduler to update all the data
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Data structure
+    mavlink_rocket_stats_tm_t stats;
+
+    // Update mutex
+    miosix::FastMutex mutex;
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/PinHandler/PinData.h b/src/boards/Main/PinHandler/PinData.h
new file mode 100644
index 0000000000000000000000000000000000000000..a100872330b1265659c74462e1c310cdd37b8245
--- /dev/null
+++ b/src/boards/Main/PinHandler/PinData.h
@@ -0,0 +1,53 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+
+namespace Main
+{
+
+struct PinChangeData
+{
+    uint64_t timestamp    = 0;
+    uint8_t pinID         = 0;
+    uint32_t changesCount = 0;
+
+    PinChangeData(uint64_t timestamp, uint8_t pinID, uint32_t changesCount)
+        : timestamp(timestamp), pinID(pinID), changesCount(changesCount)
+    {
+    }
+
+    PinChangeData() : PinChangeData{0, 0, 0} {}
+
+    static std::string header() { return "timestamp,pinID,changesCount"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << pinID << "," << changesCount << "\n";
+    }
+};
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/PinHandler/PinHandler.cpp b/src/boards/Main/PinHandler/PinHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b882c20ba0ac44a7035e5ca577e0dc0e02662ef8
--- /dev/null
+++ b/src/boards/Main/PinHandler/PinHandler.cpp
@@ -0,0 +1,145 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Configs/PinHandlerConfig.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
+
+#include <functional>
+
+using namespace Boardcore;
+using namespace std;
+using namespace std::placeholders;
+using namespace Common;
+
+namespace Main
+{
+PinHandler::PinHandler()
+{
+    PinObserver::getInstance().registerPinCallback(
+        miosix::gpios::liftoff_detach::getPin(),
+        bind(&PinHandler::onLaunchPinTransition, this, _1),
+        PinHandlerConfig::LAUNCH_PIN_THRESHOLD);
+
+    PinObserver::getInstance().registerPinCallback(
+        miosix::gpios::nosecone_detach::getPin(),
+        bind(&PinHandler::onNoseconeTransition, this, _1),
+        PinHandlerConfig::NC_DETACH_PIN_THRESHOLD);
+
+    PinObserver::getInstance().registerPinCallback(
+        miosix::gpios::cut_sense::getPin(),
+        bind(&PinHandler::onCutterSenseTransition, this, _1),
+        PinHandlerConfig::CUTTER_SENSE_PIN_THRESHOLD);
+
+    PinObserver::getInstance().registerPinCallback(
+        miosix::gpios::exp_sense::getPin(),
+        bind(&PinHandler::onExpulsionSenseTransition, this, _1),
+        PinHandlerConfig::EXPULSION_PIN_THRESHOLD);
+}
+
+bool PinHandler::start() { return PinObserver::getInstance().start(); }
+
+bool PinHandler::isStarted() { return PinObserver::getInstance().isRunning(); }
+
+void PinHandler::onLaunchPinTransition(PinTransition transition)
+{
+    if (transition == PinHandlerConfig::LAUNCH_PIN_TRIGGER)
+    {
+        EventBroker::getInstance().post(FLIGHT_LAUNCH_PIN_DETACHED,
+                                        TOPIC_FLIGHT);
+
+        // Log the event
+        PinData data = getPinData(PinList::LAUNCH_PIN);
+        PinChangeData log{TimestampTimer::getTimestamp(), PinList::LAUNCH_PIN,
+                          data.changesCount};
+        Logger::getInstance().log(log);
+    }
+}
+
+void PinHandler::onNoseconeTransition(PinTransition transition)
+{
+    if (transition == PinHandlerConfig::NC_DETACH_PIN_TRIGGER)
+    {
+        EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT);
+
+        // Log the event
+        PinData data = getPinData(PinList::NOSECONE_PIN);
+        PinChangeData log{TimestampTimer::getTimestamp(), PinList::NOSECONE_PIN,
+                          data.changesCount};
+        Logger::getInstance().log(log);
+    }
+}
+
+void PinHandler::onCutterSenseTransition(PinTransition transition)
+{
+    // Log the event
+    PinData data = getPinData(PinList::CUTTER_PRESENCE);
+    PinChangeData log{TimestampTimer::getTimestamp(), PinList::CUTTER_PRESENCE,
+                      data.changesCount};
+    Logger::getInstance().log(log);
+}
+
+void PinHandler::onExpulsionSenseTransition(PinTransition transition)
+{
+    // Log the event
+    PinData data = getPinData(PinList::PIN_EXPULSION);
+    PinChangeData log{TimestampTimer::getTimestamp(), PinList::PIN_EXPULSION,
+                      data.changesCount};
+    Logger::getInstance().log(log);
+}
+
+PinData PinHandler::getPinData(PinList pin)
+{
+    switch (pin)
+    {
+        case PinList::LAUNCH_PIN:
+        {
+            return PinObserver::getInstance().getPinData(
+                miosix::gpios::liftoff_detach::getPin());
+        }
+        case PinList::NOSECONE_PIN:
+        {
+            return PinObserver::getInstance().getPinData(
+                miosix::gpios::nosecone_detach::getPin());
+        }
+        case PinList::CUTTER_PRESENCE:
+        {
+            return PinObserver::getInstance().getPinData(
+                miosix::gpios::cut_sense::getPin());
+        }
+        case PinList::PIN_EXPULSION:
+        {
+            return PinObserver::getInstance().getPinData(
+                miosix::gpios::exp_sense::getPin());
+        }
+        default:
+        {
+            LOG_ERR(logger, "Requested non valid pin");
+            return PinData{};
+        }
+    }
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/PinHandler/PinHandler.h b/src/boards/Main/PinHandler/PinHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3f1f1275c2263664e7a6a9a44430df1749b880d
--- /dev/null
+++ b/src/boards/Main/PinHandler/PinHandler.h
@@ -0,0 +1,85 @@
+/* Copyright (c) 2019-2023 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/PinHandler/PinData.h>
+#include <diagnostic/PrintLogger.h>
+#include <utils/PinObserver/PinObserver.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class PinHandler : public Boardcore::Module
+{
+public:
+    enum PinList : uint8_t
+    {
+        LAUNCH_PIN = 0,
+        NOSECONE_PIN,
+        CUTTER_PRESENCE,
+        PIN_EXPULSION,
+        PIN_END
+    };
+
+    PinHandler();
+
+    /**
+     * @brief Starts the PinObserver module thread
+     */
+    bool start();
+
+    /**
+     * @brief Checks if the module has started correctly
+     */
+    bool isStarted();
+
+    /**
+     * @brief Called when the launch pin detaches
+     */
+    void onLaunchPinTransition(Boardcore::PinTransition transition);
+
+    /**
+     * @brief Called when the nosecone pin detaches
+     */
+    void onNoseconeTransition(Boardcore::PinTransition transition);
+
+    /**
+     * @brief Called when the cutter igniter ignites
+     */
+    void onCutterSenseTransition(Boardcore::PinTransition transition);
+
+    /**
+     * @brief Called when the expulsion sensor changes state
+     */
+    void onExpulsionSenseTransition(Boardcore::PinTransition transition);
+
+    /**
+     * @brief Returns the status of the requested pin
+     */
+    Boardcore::PinData getPinData(PinList pin);
+
+private:
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73e8afe6131d9610837259c552c72e21389b593c
--- /dev/null
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -0,0 +1,491 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#include <Main/Actuators/Actuators.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/Buses.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <events/EventBroker.h>
+#include <radio/SX1278/SX1278Frontends.h>
+
+#include <Eigen/Core>
+
+using namespace Boardcore;
+using namespace Common;
+using namespace Eigen;
+
+#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl
+#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
+#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl
+
+void __attribute__((used)) SX1278_IRQ_DIO0()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO1()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) SX1278_IRQ_DIO3()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<Main::Radio>()->transceiver)
+    {
+        modules.get<Main::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+namespace Main
+{
+
+Radio::Radio(TaskScheduler* sched) : scheduler(sched) {}
+
+bool Radio::start()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Config the transceiver
+    SX1278Fsk::Config config;
+    config.freq_rf    = 419000000;
+    config.freq_dev   = 50000;
+    config.bitrate    = 48000;
+    config.rx_bw      = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
+    config.afc_bw     = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000;
+    config.ocp        = 120;
+    config.power      = 13;
+    config.shaping    = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0;
+    config.dc_free    = Boardcore::SX1278Fsk::Config::DcFree::WHITENING;
+    config.enable_crc = false;
+
+    std::unique_ptr<SX1278::ISX1278Frontend> frontend =
+        std::make_unique<Skyward433Frontend>();
+
+    transceiver = new SX1278Fsk(
+        modules.get<Buses>()->spi6, miosix::radio::cs::getPin(),
+        miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(),
+        miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64,
+        std::move(frontend));
+
+    // Config the radio
+    SX1278Fsk::Error error = transceiver->init(config);
+
+    // Add periodic telemetry send task
+    uint8_t result1 =
+        scheduler->addTask([&]() { this->sendPeriodicMessage(); },
+                           RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
+                           TaskScheduler::Policy::RECOVER);
+    uint8_t result2 = scheduler->addTask(
+        [&]()
+        {
+            this->enqueueMsg(
+                modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0));
+        },
+        RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2,
+        TaskScheduler::Policy::RECOVER);
+
+    // Config mavDriver
+    mavDriver = new MavDriver(
+        transceiver,
+        [=](MavDriver*, const mavlink_message_t& msg)
+        { this->handleMavlinkMessage(msg); },
+        RadioConfig::RADIO_SLEEP_AFTER_SEND,
+        RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
+
+    // Check radio failure
+    if (error != SX1278Fsk::Error::NONE)
+    {
+        return false;
+    }
+
+    // Start the mavlink driver thread
+    return mavDriver->start() && result1 != 0 && result2 != 0;
+}
+
+void Radio::sendAck(const mavlink_message_t& msg)
+{
+    mavlink_message_t ackMsg;
+    mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+                            RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid,
+                            msg.seq);
+    enqueueMsg(ackMsg);
+}
+
+void Radio::sendNack(const mavlink_message_t& msg)
+{
+    mavlink_message_t nackMsg;
+    mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
+                             RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
+                             msg.seq);
+    enqueueMsg(nackMsg);
+}
+
+void Radio::logStatus() {}
+
+bool Radio::isStarted()
+{
+    return mavDriver->isStarted() && scheduler->isRunning();
+}
+
+void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    switch (msg.msgid)
+    {
+        case MAVLINK_MSG_ID_PING_TC:
+        {
+            // Do nothing, just add the ack to the queue
+            break;
+        }
+        case MAVLINK_MSG_ID_COMMAND_TC:
+        {
+            // Let the handle command reply to the message
+            return handleCommand(msg);
+        }
+        case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
+        {
+            SystemTMList tmId = static_cast<SystemTMList>(
+                mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
+
+            if (tmId == MAV_PIN_OBS_ID)
+            {
+                for (uint8_t i = 0; i < PinHandler::PIN_END; i++)
+                {
+                    mavlink_message_t msg;
+                    mavlink_pin_tm_t tm;
+
+                    PinData data = modules.get<PinHandler>()->getPinData(
+                        static_cast<PinHandler::PinList>(i));
+
+                    tm.changes_counter       = data.changesCount;
+                    tm.current_state         = tm.current_state;
+                    tm.last_change_timestamp = tm.last_change_timestamp;
+                    tm.pin_id                = tm.pin_id;
+                    tm.timestamp             = TimestampTimer::getTimestamp();
+
+                    mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                              RadioConfig::MAV_COMP_ID, &msg,
+                                              &tm);
+                    enqueueMsg(msg);
+                }
+            }
+            else if (tmId == MAV_SENSORS_STATE_ID)
+            {
+                auto sensorsState = modules.get<Sensors>()->getSensorInfo();
+
+                for (uint8_t i = 0; i < sensorsState.size(); i++)
+                {
+                    mavlink_message_t msg;
+                    mavlink_sensor_state_tm_t tm;
+
+                    strcpy(tm.sensor_name, sensorsState.at(i).id.c_str());
+                    tm.state = sensorsState.at(i).isInitialized;
+
+                    mavlink_msg_sensor_state_tm_encode(
+                        RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
+                        &msg, &tm);
+                    enqueueMsg(msg);
+                }
+            }
+            else
+            {
+                // Add to the queue the respose
+                mavlink_message_t response =
+                    modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid,
+                                                              msg.seq);
+                // Add the response to the queue
+                enqueueMsg(response);
+
+                // Check if the TM repo answered with a NACK. If so the function
+                // must return to avoid sending a default ack
+                if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+                {
+                    return;
+                }
+            }
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
+        {
+            SensorsTMList sensorId = static_cast<SensorsTMList>(
+                mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
+            mavlink_message_t response =
+                modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid,
+                                                           msg.seq);
+            enqueueMsg(response);
+
+            // If the response is a nack the method returns
+            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+            {
+                return;
+            }
+            break;
+        }
+        case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
+        {
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
+            mavlink_message_t response =
+                modules.get<TMRepository>()->packServoTm(servo, msg.msgid,
+                                                         msg.seq);
+            enqueueMsg(response);
+
+            // If the response is a nack the method returns
+            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+            {
+                return;
+            }
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
+            float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // If the state is test mode, the servo is set to the correct angle
+            modules.get<Actuators>()->setServoPosition(servoId, position);
+            break;
+        }
+        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // If the state is test mode, the wiggle is done
+            modules.get<Actuators>()->wiggleServo(servoId);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_RESET_SERVO_TC:
+        {
+            ServosList servoId = static_cast<ServosList>(
+                mavlink_msg_reset_servo_tc_get_servo_id(&msg));
+
+            // Send nack if the FMM is not in test mode
+            if (!modules.get<FlightModeManager>()->testState(
+                    &FlightModeManager::state_test_mode))
+            {
+                return sendNack(msg);
+            }
+
+            // Set the servo position to 0
+            modules.get<Actuators>()->setServoPosition(servoId, 0);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
+        {
+            float altitude =
+                mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
+
+            modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
+        {
+            float altitude =
+                mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
+
+            modules.get<ADAController>()->setReferenceAltitude(altitude);
+            modules.get<NASController>()->setReferenceAltitude(altitude);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
+        {
+            float temperature =
+                mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
+
+            modules.get<ADAController>()->setReferenceTemperature(temperature);
+            modules.get<NASController>()->setReferenceTemperature(temperature);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_COORDINATES_TC:
+        {
+            float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
+            float longitude =
+                mavlink_msg_set_coordinates_tc_get_longitude(&msg);
+
+            Vector2f coordinates{latitude, longitude};
+
+            modules.get<NASController>()->setCoordinates(coordinates);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_ORIENTATION_TC:
+        {
+            float yaw   = mavlink_msg_set_orientation_tc_get_yaw(&msg);
+            float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg);
+            float roll  = mavlink_msg_set_orientation_tc_get_roll(&msg);
+
+            modules.get<NASController>()->setOrientation(yaw, pitch, roll);
+            break;
+        }
+        default:
+        {
+            LOG_DEBUG(logger, "Received message is not of a known type");
+            return sendNack(msg);
+        }
+    }
+
+    // At the end send the ack message
+    sendAck(msg);
+}
+
+void Radio::handleCommand(const mavlink_message_t& msg)
+{
+    MavCommandList commandId = static_cast<MavCommandList>(
+        mavlink_msg_command_tc_get_command_id(&msg));
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Create the map between the commands and the corresponding events
+    static const std::map<MavCommandList, Events> commandToEvent{
+        {MAV_CMD_ARM, TMTC_ARM},
+        {MAV_CMD_DISARM, TMTC_DISARM},
+        {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
+        {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
+        {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
+        {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
+        {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
+        {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
+        {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
+        {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
+        {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE},
+        {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE},
+        {MAV_CMD_START_RECORDING, TMTC_START_RECORDING},
+        {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING},
+    };
+
+    switch (commandId)
+    {
+        case MAV_CMD_SAVE_CALIBRATION:
+        {
+            // Save the sensor calibration and adopt it
+            modules.get<Sensors>()->writeMagCalibration();
+            break;
+        }
+        case MAV_CMD_START_LOGGING:
+        {
+            bool result = Logger::getInstance().start();
+
+            // In case the logger is not started send to GS the result
+            if (!result)
+            {
+                return sendNack(msg);
+            }
+            break;
+        }
+        case MAV_CMD_STOP_LOGGING:
+        {
+            Logger::getInstance().stop();
+            break;
+        }
+        default:
+        {
+            // If the command is not a particular one, look for it inside the
+            // map
+            auto it = commandToEvent.find(commandId);
+
+            if (it != commandToEvent.end())
+            {
+                EventBroker::getInstance().post(it->second, TOPIC_TMTC);
+            }
+            else
+            {
+                return sendNack(msg);
+            }
+        }
+    }
+    // Acknowledge the message
+    sendAck(msg);
+}
+
+void Radio::sendPeriodicMessage()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Send all the queue messages
+    {
+        Lock<FastMutex> lock(queueMutex);
+
+        for (uint8_t i = 0; i < messageQueueIndex; i++)
+        {
+            mavDriver->enqueueMsg(messageQueue[i]);
+        }
+
+        // Reset the index
+        messageQueueIndex = 0;
+    }
+
+    mavDriver->enqueueMsg(
+        modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0));
+    mavDriver->enqueueMsg(
+        modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0));
+}
+
+void Radio::enqueueMsg(const mavlink_message_t& msg)
+{
+    {
+        Lock<FastMutex> lock(queueMutex);
+
+        // Insert the message inside the queue only if there is enough space
+        if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE)
+        {
+            messageQueue[messageQueueIndex] = msg;
+            messageQueueIndex++;
+        }
+    }
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c1fd4cde31fcbde4b190b31c0e3c56291b728e6
--- /dev/null
+++ b/src/boards/Main/Radio/Radio.h
@@ -0,0 +1,102 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Main/Configs/RadioConfig.h>
+#include <common/Mavlink.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SX1278/SX1278Fsk.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
+                                           RadioConfig::RADIO_OUT_QUEUE_SIZE,
+                                           RadioConfig::RADIO_MAV_MSG_LENGTH>;
+
+class Radio : public Boardcore::Module
+{
+public:
+    Radio(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the MavlinkDriver
+     */
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Sends via radio an acknowledge message about the parameter passed
+     * message
+     */
+    void sendAck(const mavlink_message_t& msg);
+
+    /**
+     * @brief Sends via radio an non-acknowledge message about the parameter
+     * passed message
+     */
+    void sendNack(const mavlink_message_t& msg);
+
+    /**
+     * @brief Saves the MavlinkDriver and transceiver status
+     */
+    void logStatus();
+
+    /**
+     * @brief Returns if the radio module is correctly started
+     */
+    bool isStarted();
+
+    Boardcore::SX1278Fsk* transceiver = nullptr;
+    MavDriver* mavDriver              = nullptr;
+
+private:
+    /**
+     * @brief Called by the MavlinkDriver when a message is received
+     */
+    void handleMavlinkMessage(const mavlink_message_t& msg);
+
+    /**
+     * @brief Called by the handleMavlinkMessage to handle a command message
+     */
+    void handleCommand(const mavlink_message_t& msg);
+
+    /**
+     * @brief Sends the periodic telemetry
+     */
+    void sendPeriodicMessage();
+
+    /**
+     * @brief Inserts the mavlink message into the queue
+     */
+    void enqueueMsg(const mavlink_message_t& msg);
+
+    // Messages queue
+    mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE];
+    uint32_t messageQueueIndex = 0;
+    miosix::FastMutex queueMutex;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
+    Boardcore::TaskScheduler* scheduler = nullptr;
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2bee48f748fe0a98661f009f1e427432e2c04233
--- /dev/null
+++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
@@ -0,0 +1,103 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Sensors/RotatedIMU/RotatedIMU.h>
+
+using namespace Eigen;
+using namespace Boardcore;
+
+namespace Main
+{
+
+RotatedIMU::RotatedIMU(std::function<AccelerometerData()> accSampleFunction,
+                       std::function<MagnetometerData()> magSampleFunction,
+                       std::function<GyroscopeData()> gyroSampleFunction)
+    : accSample(accSampleFunction), magSample(magSampleFunction),
+      gyroSample(gyroSampleFunction), accT(Matrix3f::Identity()),
+      magT(Matrix3f::Identity()), gyroT(Matrix3f::Identity())
+{
+}
+
+void RotatedIMU::addAccTransformation(const Matrix3f& t) { accT = t * accT; }
+void RotatedIMU::addMagTransformation(const Matrix3f& t) { magT = t * magT; }
+void RotatedIMU::addGyroTransformation(const Matrix3f& t) { gyroT = t * gyroT; }
+
+void RotatedIMU::resetTransformations()
+{
+    accT  = Matrix3f::Identity();
+    magT  = Matrix3f::Identity();
+    gyroT = Matrix3f::Identity();
+}
+
+RotatedIMUData RotatedIMU::sampleImpl()
+{
+    Vector3f accResult  = accT * static_cast<Vector3f>(accSample());
+    Vector3f magResult  = magT * static_cast<Vector3f>(magSample());
+    Vector3f gyroResult = gyroT * static_cast<Vector3f>(gyroSample());
+
+    return {AccelerometerData{accResult}, GyroscopeData{gyroResult},
+            MagnetometerData{magResult}};
+}
+
+// Static functions
+Matrix3f RotatedIMU::rotateAroundX(float angle)
+{
+    Matrix3f rotation;
+    angle = angle * EIGEN_PI / 180.f;
+
+    // clang-format off
+    rotation = Matrix3f{{1,     0,           0},
+                        {0,     cosf(angle), -sinf(angle)},
+                        {0,     sinf(angle), cosf(angle)}};
+    // clang-format on
+
+    return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundY(float angle)
+{
+    Matrix3f rotation;
+    angle = angle * EIGEN_PI / 180.f;
+
+    // clang-format off
+    rotation = Matrix3f{{cosf(angle),   0,  sinf(angle)},
+                        {0,             1,  0},
+                        {-sinf(angle),  0,  cosf(angle)}};
+    // clang-format on
+
+    return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundZ(float angle)
+{
+    Matrix3f rotation;
+    angle = angle * EIGEN_PI / 180.f;
+
+    // clang-format off
+    rotation = Matrix3f{{cosf(angle),   -sinf(angle),   0},
+                        {sinf(angle),   cosf(angle),    0},
+                        {0,             0,              1}};
+    // clang-format on
+
+    return rotation;
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc26b01524f2764e00bdc1e2193d2698f8ee123f
--- /dev/null
+++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h
@@ -0,0 +1,120 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Main/Sensors/RotatedIMU/RotatedIMUData.h>
+#include <sensors/Sensor.h>
+
+#include <Eigen/Eigen>
+#include <functional>
+
+namespace Main
+{
+/**
+ * @brief Creates a "fake" sensor which allows the user
+ * to transform the values with transformation matrices.
+ * The default configuration (thus the reset one)
+ * comprehends identity matrices as transformation ones.
+ *
+ * @warning This is NOT a thread safe class
+ */
+class RotatedIMU : public Boardcore::Sensor<RotatedIMUData>
+{
+public:
+    /**
+     * @brief Construct a new Rotated IMU object
+     *
+     * @param accSampleFunction Function to call to retrieve an accelerometer
+     * sample
+     * @param magSampleFunction Function to call to retrieve a magnetometer
+     * sample
+     * @param gyroSampleFunction Function to call to retrieve a gyroscope sample
+     */
+    RotatedIMU(
+        std::function<Boardcore::AccelerometerData()> accSampleFunction = []()
+        { return Boardcore::AccelerometerData{}; },
+        std::function<Boardcore::MagnetometerData()> magSampleFunction = []()
+        { return Boardcore::MagnetometerData{}; },
+        std::function<Boardcore::GyroscopeData()> gyroSampleFunction = []()
+        { return Boardcore::GyroscopeData{}; });
+
+    /**
+     * @brief Multiplies the current accelerometer transformation
+     * @param transformation Transformation matrix to be multiplied to the
+     * current one
+     */
+    void addAccTransformation(const Eigen::Matrix3f& t);
+
+    /**
+     * @brief Multiplies the current magnetometer transformation
+     * @param transformation Transformation matrix to be multiplied to the
+     * current one
+     */
+    void addMagTransformation(const Eigen::Matrix3f& t);
+
+    /**
+     * @brief Multiplies the current gyroscope transformation
+     * @param transformation Transformation matrix to be multiplied to the
+     * current one
+     */
+    void addGyroTransformation(const Eigen::Matrix3f& t);
+
+    /**
+     * @brief Resets all the transformations to the original (Identity) ones
+     */
+    void resetTransformations();
+
+    bool init() override { return true; }
+    bool selfTest() override { return true; }
+
+    /**
+     * @brief Creates a rotation matrix around the X axis
+     * @param angle Angle in degrees
+     */
+    static Eigen::Matrix3f rotateAroundX(float angle);
+
+    /**
+     * @brief Creates a rotation matrix around the Y axis
+     * @param angle Angle in degrees
+     */
+    static Eigen::Matrix3f rotateAroundY(float angle);
+
+    /**
+     * @brief Creates a rotation matrix around the Z axis
+     * @param angle Angle in degrees
+     */
+    static Eigen::Matrix3f rotateAroundZ(float angle);
+
+private:
+    RotatedIMUData sampleImpl() override;
+
+    // Functions to sample the under neath sensors
+    std::function<Boardcore::AccelerometerData()> accSample;
+    std::function<Boardcore::MagnetometerData()> magSample;
+    std::function<Boardcore::GyroscopeData()> gyroSample;
+
+    // Transformation matrices
+    Eigen::Matrix3f accT;
+    Eigen::Matrix3f magT;
+    Eigen::Matrix3f gyroT;
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h b/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h
new file mode 100644
index 0000000000000000000000000000000000000000..48a6fbcd4fc20185828379d46fc396a4c4a6affc
--- /dev/null
+++ b/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <sensors/SensorData.h>
+
+struct RotatedIMUData : public Boardcore::AccelerometerData,
+                        public Boardcore::MagnetometerData,
+                        public Boardcore::GyroscopeData
+{
+    RotatedIMUData()
+        : AccelerometerData{0, 0.0, 0.0, 0.0},
+          MagnetometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+
+    {
+    }
+
+    RotatedIMUData(AccelerometerData acc, GyroscopeData gyr,
+                   MagnetometerData mag)
+        : AccelerometerData(acc), MagnetometerData(mag), GyroscopeData(gyr)
+    {
+    }
+
+    static std::string header()
+    {
+        return "accelerationTimestamp,accelerationX,accelerationY,"
+               "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+               "angularSpeedY,angularSpeedZ,magneticFieldTimestamp,"
+               "magneticFieldX,magneticFieldY,magneticFieldZ\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << accelerationTimestamp << "," << accelerationX << ","
+           << accelerationY << "," << accelerationZ << ","
+           << angularSpeedTimestamp << "," << angularSpeedX << ","
+           << angularSpeedY << "," << angularSpeedZ << ","
+           << magneticFieldTimestamp << "," << magneticFieldX << ","
+           << magneticFieldY << "," << magneticFieldZ << "\n";
+    }
+};
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..572a222201f46ce7e3f422eb11b7a4eeb5418bb3
--- /dev/null
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -0,0 +1,731 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Sensors.h"
+
+#include <Main/Buses.h>
+#include <Main/Configs/SensorsConfig.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace Boardcore;
+using namespace std;
+using namespace Main::SensorsConfig;
+namespace Main
+{
+LPS22DFData Sensors::getLPS22DFLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_1LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{};
+}
+LPS28DFWData Sensors::getLPS28DFW_2LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{};
+}
+H3LIS331DLData Sensors::getH3LIS331DLLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return h3lis331dl != nullptr ? h3lis331dl->getLastSample()
+                                 : H3LIS331DLData{};
+}
+LIS2MDLData Sensors::getLIS2MDLLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{};
+}
+UBXGPSData Sensors::getGPSLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{};
+}
+LSM6DSRXData Sensors::getLSM6DSRXLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{};
+}
+ADS131M08Data Sensors::getADS131M08LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+}
+
+PitotData Sensors::getPitotLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canPitot;
+}
+PressureData Sensors::getCCPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canCCPressure;
+}
+PressureData Sensors::getBottomTankPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canBottomTankPressure;
+}
+PressureData Sensors::getTopTankPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canTopTankPressure;
+}
+TemperatureData Sensors::getTankTemperatureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return canTankTemperature;
+}
+BatteryVoltageSensorData Sensors::getMotorBatteryVoltage()
+{
+    miosix::PauseKernelLock lock;
+    return canMotorBatteryVoltage;
+}
+CurrentData Sensors::getMotorCurrent()
+{
+    miosix::PauseKernelLock lock;
+    return canMotorCurrent;
+}
+
+// Processed Getters
+BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    BatteryVoltageSensorData data;
+
+    // Populate the data
+    data.voltageTimestamp = sample.timestamp;
+    data.channelId        = 1;
+    data.voltage          = sample.voltage[1];
+    data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
+    return data;
+}
+
+BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    BatteryVoltageSensorData data;
+
+    // Populate the data
+    data.voltageTimestamp = sample.timestamp;
+    data.channelId        = 7;
+    data.voltage          = sample.voltage[7];
+    data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR;
+    return data;
+}
+
+CurrentData Sensors::getCurrentLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    ADS131M08Data sample = getADS131M08LastSample();
+    CurrentData data;
+
+    // Populate the data
+    data.currentTimestamp = sample.timestamp;
+    data.current =
+        (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR;
+    return data;
+}
+
+MPXH6400AData Sensors::getDeploymentPressureLastSample()
+{
+    miosix::PauseKernelLock lock;
+    return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{};
+}
+
+HSCMRNN015PAData Sensors::getStaticPressure1LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample()
+                                     : HSCMRNN015PAData{};
+}
+
+HSCMRNN015PAData Sensors::getStaticPressure2LastSample()
+{
+    miosix::PauseKernelLock lock;
+    return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample()
+                                     : HSCMRNN015PAData{};
+}
+
+RotatedIMUData Sensors::getIMULastSample()
+{
+    miosix::PauseKernelLock lock;
+    return imu != nullptr ? imu->getLastSample() : RotatedIMUData{};
+}
+
+MagnetometerData Sensors::getCalibratedMagnetometerLastSample()
+{
+    // Do not need to pause the kernel, the last sample getter is already
+    // protected
+    MagnetometerData lastSample = getLIS2MDLLastSample();
+    MagnetometerData result;
+
+    // Correct the result and copy the timestamp
+    {
+        miosix::Lock<FastMutex> l(calibrationMutex);
+        result =
+            static_cast<MagnetometerData>(magCalibration.correct(lastSample));
+    }
+
+    result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp;
+    return result;
+}
+
+void Sensors::setPitot(PitotData data)
+{
+    miosix::PauseKernelLock lock;
+    canPitot.timestamp = TimestampTimer::getTimestamp();
+    canPitot           = data;
+}
+void Sensors::setCCPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+    canCCPressure                   = data;
+}
+void Sensors::setBottomTankPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+    canBottomTankPressure                   = data;
+}
+void Sensors::setTopTankPressure(PressureData data)
+{
+    miosix::PauseKernelLock lock;
+    canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+    canTopTankPressure                   = data;
+}
+void Sensors::setTankTemperature(TemperatureData data)
+{
+    miosix::PauseKernelLock lock;
+    canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp();
+    canTankTemperature                      = data;
+}
+void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data)
+{
+    miosix::PauseKernelLock lock;
+    canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp();
+    canMotorBatteryVoltage.batVoltage       = data.batVoltage;
+}
+void Sensors::setMotorCurrent(CurrentData data)
+{
+    miosix::PauseKernelLock lock;
+    canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp();
+    canMotorCurrent.current          = data.current;
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {}
+
+bool Sensors::start()
+{
+    // Read the magnetometer calibration from predefined file
+    magCalibration.fromFile("magCalibration.csv");
+
+    // Init all the sensors
+    lps22dfInit();
+    lps28dfw_1Init();
+    // lps28dfw_2Init();
+    h3lis331dlInit();
+    lis2mdlInit();
+    ubxgpsInit();
+    lsm6dsrxInit();
+    ads131m08Init();
+    deploymentPressureInit();
+    staticPressure1Init();
+    staticPressure2Init();
+    imuInit();
+
+    // Add the magnetometer calibration to the scheduler
+    size_t result = scheduler->addTask(
+        [&]()
+        {
+            // Gather the last sample data
+            MagnetometerData lastSample = getLIS2MDLLastSample();
+
+            // Feed the data to the calibrator inside a protected area.
+            // Contention is not high and the use of a mutex is suitable to
+            // avoid pausing the kernel for this calibration operation
+            {
+                miosix::Lock<FastMutex> l(calibrationMutex);
+                magCalibrator.feed(lastSample);
+            }
+        },
+        MAG_CALIBRATION_PERIOD);
+
+    // Create sensor manager with populated map and configured scheduler
+    manager = new SensorManager(sensorMap, scheduler);
+    return manager->start() && result != 0;
+}
+
+void Sensors::stop() { manager->stop(); }
+
+bool Sensors::isStarted()
+{
+    return manager->areAllSensorsInitialized() && scheduler->isRunning();
+}
+
+void Sensors::calibrate()
+{
+    // Create the stats to calibrate the barometers
+    Stats lps28dfw1Stats;
+    // Stats lps28dfw2Stats;
+    Stats staticPressure1Stats;
+    Stats staticPressure2Stats;
+    Stats deploymentPressureStats;
+
+    // Add N samples to the stats
+    for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++)
+    {
+        lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure);
+        // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure);
+        staticPressure1Stats.add(getStaticPressure1LastSample().pressure);
+        staticPressure2Stats.add(getStaticPressure2LastSample().pressure);
+        deploymentPressureStats.add(getDeploymentPressureLastSample().pressure);
+
+        // Delay for the expected period
+        miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD);
+    }
+
+    // Compute the difference between the mean value from LPS28DFW
+    float reference = lps28dfw1Stats.getStats().mean;
+
+    hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean -
+                                 reference);
+    hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean -
+                                 reference);
+    mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean -
+                            reference);
+
+    // Log the offsets
+    SensorsCalibrationParameter cal{};
+    cal.timestamp         = TimestampTimer::getTimestamp();
+    cal.offsetStatic1     = staticPressure1Stats.getStats().mean - reference;
+    cal.offsetStatic2     = staticPressure2Stats.getStats().mean - reference;
+    cal.offsetDeployment  = deploymentPressureStats.getStats().mean - reference;
+    cal.referencePressure = reference;
+
+    Logger::getInstance().log(cal);
+}
+
+void Sensors::writeMagCalibration()
+{
+    // Compute the calibration result in protected area
+    {
+        miosix::Lock<FastMutex> l(calibrationMutex);
+        SixParametersCorrector cal = magCalibrator.computeResult();
+
+        // Check result validity
+        if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) &&
+            !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) &&
+            !isnan(cal.getA()[1]) && !isnan(cal.getA()[2]))
+        {
+            magCalibration = cal;
+
+            // Save the calibration to the calibration file
+            magCalibration.toFile("magCalibration.csv");
+        }
+    }
+}
+
+std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+Sensors::getSensorInfo()
+{
+    std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState;
+    for (size_t i = 0; i < sensorsInit.size(); i++)
+    {
+        // Check wether the lambda is existent
+        if (sensorsInit[i])
+        {
+            sensorState[i] = sensorsInit[i]();
+        }
+    }
+    return sensorState;
+}
+
+void Sensors::lps22dfInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = LPS22DF::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Configure the device
+    LPS22DF::Config sensorConfig;
+    sensorConfig.avg = LPS22DF_AVG;
+    sensorConfig.odr = LPS22DF_ODR;
+
+    // Create sensor instance with configured parameters
+    lps22df = new LPS22DF(modules.get<Buses>()->spi3,
+                          miosix::sensors::LPS22DF::cs::getPin(), config,
+                          sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS22DF", LPS22DF_PERIOD,
+                    bind(&Sensors::lps22dfCallback, this));
+    sensorMap.emplace(make_pair(lps22df, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps22df); };
+}
+void Sensors::lps28dfw_1Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD,
+                    bind(&Sensors::lps28dfw_1Callback, this));
+    sensorMap.emplace(make_pair(lps28dfw_1, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps28dfw_1); };
+}
+void Sensors::lps28dfw_2Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the sensor
+    LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG,
+                                  LPS28DFW_ODR, false};
+
+    // Create sensor instance with configured parameters
+    lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD,
+                    bind(&Sensors::lps28dfw_2Callback, this));
+    sensorMap.emplace(make_pair(lps28dfw_2, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lps28dfw_2); };
+}
+void Sensors::h3lis331dlInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Create sensor instance with configured parameters
+    h3lis331dl = new H3LIS331DL(
+        modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(),
+        config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD,
+                    bind(&Sensors::h3lis331dlCallback, this));
+    sensorMap.emplace(make_pair(h3lis331dl, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(h3lis331dl); };
+}
+void Sensors::lis2mdlInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = LIS2MDL::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_16;
+
+    // Configure the sensor
+    LIS2MDL::Config sensorConfig;
+    sensorConfig.deviceMode         = LIS2MDL_OPERATIVE_MODE;
+    sensorConfig.odr                = LIS2MDL_ODR;
+    sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER;
+
+    // Create sensor instance with configured parameters
+    lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3,
+                          miosix::sensors::LIS2MDL::cs::getPin(), config,
+                          sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LIS2MDL", LIS2MDL_PERIOD,
+                    bind(&Sensors::lis2mdlCallback, this));
+    sensorMap.emplace(make_pair(lis2mdl, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lis2mdl); };
+}
+
+void Sensors::ubxgpsInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Get the correct SPI configuration
+    SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig();
+    config.clockDivider = SPI::ClockDivider::DIV_64;
+
+    // Create sensor instance with configured parameters
+    ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4,
+                           miosix::sensors::GPS::cs::getPin(), config, 5);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("UBXGPS", UBXGPS_PERIOD,
+                    bind(&Sensors::ubxgpsCallback, this));
+    sensorMap.emplace(make_pair(ubxgps, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(ubxgps); };
+}
+
+void Sensors::lsm6dsrxInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the SPI
+    SPIBusConfig config;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+    config.mode         = SPI::Mode::MODE_0;
+
+    // Configure the sensor
+    LSM6DSRXConfig sensorConfig;
+    sensorConfig.bdu = LSM6DSRX_BDU;
+
+    // Accelerometer
+    sensorConfig.fsAcc     = LSM6DSRX_ACC_FS;
+    sensorConfig.odrAcc    = LSM6DSRX_ACC_ODR;
+    sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE;
+
+    // Gyroscope
+    sensorConfig.fsGyr     = LSM6DSRX_GYR_FS;
+    sensorConfig.odrGyr    = LSM6DSRX_GYR_ODR;
+    sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE;
+
+    // Fifo
+    sensorConfig.fifoMode                = LSM6DSRX_FIFO_MODE;
+    sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION;
+    sensorConfig.fifoTemperatureBdr      = LSM6DSRX_FIFO_TEMPERATURE_BDR;
+
+    // Create sensor instance with configured parameters
+    lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1,
+                            miosix::sensors::LSM6DSRX::cs::getPin(), config,
+                            sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD,
+                    bind(&Sensors::lsm6dsrxCallback, this));
+    sensorMap.emplace(make_pair(lsm6dsrx, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(lsm6dsrx); };
+}
+
+void Sensors::ads131m08Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Configure the SPI
+    SPIBusConfig config;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+
+    // Configure the device
+    ADS131M08::Config sensorConfig;
+    sensorConfig.oversamplingRatio     = ADS131M08_OVERSAMPLING_RATIO;
+    sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE;
+
+    // Create the sensor instance with configured parameters
+    ads131m08 = new ADS131M08(modules.get<Buses>()->spi4,
+                              miosix::sensors::ADS131::cs::getPin(), config,
+                              sensorConfig);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("ADS131M08", ADS131M08_PERIOD,
+                    bind(&Sensors::ads131m08Callback, this));
+    sensorMap.emplace(make_pair(ads131m08, info));
+
+    // Add the sensor info getter to the array
+    sensorsInit[sensorsId++] = [&]() -> SensorInfo
+    { return manager->getSensorInfo(ads131m08); };
+}
+
+void Sensors::deploymentPressureInit()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_DPL);
+    };
+
+    // Create the sensor instance with created function
+    mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("MPXH6400A", ADS131M08_PERIOD,
+                    bind(&Sensors::deploymentPressureCallback, this));
+    sensorMap.emplace(make_pair(mpxh6400a, info));
+}
+
+void Sensors::staticPressure1Init()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_STATIC_1);
+    };
+
+    // Create the sensor instance with created function
+    hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD,
+                    bind(&Sensors::staticPressure1Callback, this));
+    sensorMap.emplace(make_pair(hscmrnn015pa_1, info));
+}
+
+void Sensors::staticPressure2Init()
+{
+    // Create the lambda function to get the voltage
+    function<ADCData()> getVoltage = [&]()
+    {
+        // No need to synchronize, the sampling thread is the same
+        ADS131M08Data sample = ads131m08->getLastSample();
+        return sample.getVoltage(ADC_CH_STATIC_2);
+    };
+
+    // Create the sensor instance with created function
+    hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD,
+                    bind(&Sensors::staticPressure2Callback, this));
+    sensorMap.emplace(make_pair(hscmrnn015pa_2, info));
+}
+
+void Sensors::imuInit()
+{
+    // Register the IMU as the fake sensor, passing as parameters the methods to
+    // retrieve real data. The sensor is not synchronized, but the sampling
+    // thread is always the same.
+    imu = new RotatedIMU(
+        bind(&LSM6DSRX::getLastSample, lsm6dsrx),
+        bind(&Sensors::getCalibratedMagnetometerLastSample, this),
+        bind(&LSM6DSRX::getLastSample, lsm6dsrx));
+
+    // Invert the Y axis on the magnetometer
+    Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}};
+    imu->addMagTransformation(m);
+
+    // Emplace the sensor inside the map
+    SensorInfo info("RotatedIMU", IMU_PERIOD,
+                    bind(&Sensors::imuCallback, this));
+    sensorMap.emplace(make_pair(imu, info));
+}
+
+void Sensors::lps22dfCallback()
+{
+    LPS22DFData lastSample = lps22df->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_1Callback()
+{
+    LPS28DFW_1Data lastSample =
+        static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lps28dfw_2Callback()
+{
+    LPS28DFW_2Data lastSample =
+        static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::h3lis331dlCallback()
+{
+    H3LIS331DLData lastSample = h3lis331dl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lis2mdlCallback()
+{
+    LIS2MDLData lastSample = lis2mdl->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::ubxgpsCallback()
+{
+    UBXGPSData lastSample = ubxgps->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::lsm6dsrxCallback()
+{
+    auto& fifo        = lsm6dsrx->getLastFifo();
+    uint16_t fifoSize = lsm6dsrx->getLastFifoSize();
+
+    // For every instance inside the fifo log the sample
+    for (uint16_t i = 0; i < fifoSize; i++)
+    {
+        Logger::getInstance().log(fifo.at(i));
+    }
+}
+void Sensors::ads131m08Callback()
+{
+    ADS131M08Data lastSample = ads131m08->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::deploymentPressureCallback()
+{
+    MPXH6400AData lastSample = mpxh6400a->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::staticPressure1Callback()
+{
+    HSCMRNN015PA_1Data lastSample =
+        static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::staticPressure2Callback()
+{
+    HSCMRNN015PA_2Data lastSample =
+        static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample());
+    Logger::getInstance().log(lastSample);
+}
+void Sensors::imuCallback()
+{
+    RotatedIMUData lastSample = imu->getLastSample();
+    Logger::getInstance().log(lastSample);
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9dafb632f1fa8aced819b5e773d551e9dbb2639
--- /dev/null
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -0,0 +1,202 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Main/Configs/SensorsConfig.h>
+#include <Main/Sensors/RotatedIMU/RotatedIMU.h>
+#include <Main/Sensors/SensorsData.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS2MDL/LIS2MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/LPS28DFW/LPS28DFW.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <sensors/SensorData.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <sensors/analog/BatteryVoltageSensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h>
+#include <sensors/analog/pressure/nxp/MPXH6400A.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+#include <stdint.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class Sensors : public Boardcore::Module
+{
+public:
+    explicit Sensors(Boardcore::TaskScheduler* sched);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Stops the sensor manager
+     * @warning Stops the passed scheduler
+     */
+    void stop();
+
+    /**
+     * @brief Returns if all the sensors are started successfully
+     */
+    bool isStarted();
+
+    /**
+     * @brief Calibrates the sensors with an offset
+     */
+    void calibrate();
+
+    /**
+     * @brief Takes the result of the live magnetometer calibration and applies
+     * it to the current calibration + writes it in the csv file
+     */
+    void writeMagCalibration();
+
+    // Sensor getters
+    Boardcore::LPS22DFData getLPS22DFLastSample();
+    Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
+    Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
+    Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+    Boardcore::LIS2MDLData getLIS2MDLLastSample();
+    Boardcore::UBXGPSData getGPSLastSample();
+    Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+    Boardcore::ADS131M08Data getADS131M08LastSample();
+
+    // Processed getters
+    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+    Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
+    Boardcore::CurrentData getCurrentLastSample();
+    Boardcore::MPXH6400AData getDeploymentPressureLastSample();
+    Boardcore::HSCMRNN015PAData getStaticPressure1LastSample();
+    Boardcore::HSCMRNN015PAData getStaticPressure2LastSample();
+    RotatedIMUData getIMULastSample();
+    Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
+
+    // CAN fake sensors setters
+    void setPitot(Boardcore::PitotData data);
+    void setCCPressure(Boardcore::PressureData data);
+    void setBottomTankPressure(Boardcore::PressureData data);
+    void setTopTankPressure(Boardcore::PressureData data);
+    void setTankTemperature(Boardcore::TemperatureData data);
+    void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data);
+    void setMotorCurrent(Boardcore::CurrentData data);
+
+    // CAN fake sensors getters
+    Boardcore::PitotData getPitotLastSample();
+    Boardcore::PressureData getCCPressureLastSample();
+    Boardcore::PressureData getBottomTankPressureLastSample();
+    Boardcore::PressureData getTopTankPressureLastSample();
+    Boardcore::TemperatureData getTankTemperatureLastSample();
+    Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage();
+    Boardcore::CurrentData getMotorCurrent();
+
+    // Returns the sensors statuses
+    std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS>
+    getSensorInfo();
+
+private:
+    // Init and callbacks methods
+    void lps22dfInit();
+    void lps22dfCallback();
+
+    void lps28dfw_1Init();
+    void lps28dfw_1Callback();
+
+    void lps28dfw_2Init();
+    void lps28dfw_2Callback();
+
+    void h3lis331dlInit();
+    void h3lis331dlCallback();
+
+    void lis2mdlInit();
+    void lis2mdlCallback();
+
+    void ubxgpsInit();
+    void ubxgpsCallback();
+
+    void lsm6dsrxInit();
+    void lsm6dsrxCallback();
+
+    void ads131m08Init();
+    void ads131m08Callback();
+
+    void deploymentPressureInit();
+    void deploymentPressureCallback();
+
+    void staticPressure1Init();
+    void staticPressure1Callback();
+
+    void staticPressure2Init();
+    void staticPressure2Callback();
+
+    void imuInit();
+    void imuCallback();
+
+    // Sensors instances
+    Boardcore::LPS22DF* lps22df       = nullptr;
+    Boardcore::LPS28DFW* lps28dfw_1   = nullptr;
+    Boardcore::LPS28DFW* lps28dfw_2   = nullptr;
+    Boardcore::H3LIS331DL* h3lis331dl = nullptr;
+    Boardcore::LIS2MDL* lis2mdl       = nullptr;
+    Boardcore::UBXGPSSpi* ubxgps      = nullptr;
+    Boardcore::LSM6DSRX* lsm6dsrx     = nullptr;
+    Boardcore::ADS131M08* ads131m08   = nullptr;
+
+    // Can sensors
+    Boardcore::PitotData canPitot{};
+    Boardcore::PressureData canCCPressure{};
+    Boardcore::PressureData canBottomTankPressure{};
+    Boardcore::PressureData canTopTankPressure{};
+    Boardcore::TemperatureData canTankTemperature{};
+    Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{};
+    Boardcore::CurrentData canMotorCurrent{};
+
+    // Fake processed sensors
+    RotatedIMU* imu                         = nullptr;
+    Boardcore::MPXH6400A* mpxh6400a         = nullptr;
+    Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr;
+    Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr;
+
+    // Magnetometer live calibration
+    Boardcore::SoftAndHardIronCalibration magCalibrator;
+    Boardcore::SixParametersCorrector magCalibration;
+    miosix::FastMutex calibrationMutex;
+
+    // Sensor manager
+    Boardcore::SensorManager* manager = nullptr;
+    Boardcore::SensorManager::SensorMap_t sensorMap;
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Collection of lambdas to get the sensor init statuses
+    std::array<std::function<Boardcore::SensorInfo()>,
+               SensorsConfig::NUMBER_OF_SENSORS>
+        sensorsInit;
+    uint8_t sensorsId = 0;
+
+    // SD logger
+    Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance();
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Sensors/SensorsData.h b/src/boards/Main/Sensors/SensorsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..eda2f6980b808a9f4678eab2514b8000b02f3931
--- /dev/null
+++ b/src/boards/Main/Sensors/SensorsData.h
@@ -0,0 +1,140 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <sensors/LPS28DFW/LPS28DFWData.h>
+#include <sensors/analog/pressure/honeywell/HSCMRNN015PAData.h>
+
+namespace Main
+{
+struct SensorsCalibrationParameter
+{
+    uint64_t timestamp;
+    float referencePressure;
+    float offsetStatic1;
+    float offsetStatic2;
+    float offsetDeployment;
+
+    SensorsCalibrationParameter(uint64_t timestamp, float referencePressure,
+                                float offsetStatic1, float offsetStatic2,
+                                float offsetDeployment)
+        : timestamp(timestamp), referencePressure(referencePressure),
+          offsetStatic1(offsetStatic1), offsetStatic2(offsetStatic2),
+          offsetDeployment(offsetDeployment)
+    {
+    }
+
+    SensorsCalibrationParameter() : SensorsCalibrationParameter(0, 0, 0, 0, 0)
+    {
+    }
+
+    static std::string header()
+    {
+        return "timestamp,referencePressure,offsetStatic1,offsetStatic2,"
+               "offsetDeployment";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << referencePressure << "," << offsetStatic1
+           << "," << offsetStatic2 << "," << offsetDeployment << "\n";
+    }
+};
+struct LPS28DFW_1Data : Boardcore::LPS28DFWData
+{
+    explicit LPS28DFW_1Data(const Boardcore::LPS28DFWData& data)
+        : Boardcore::LPS28DFWData(data)
+    {
+    }
+
+    LPS28DFW_1Data() {}
+
+    static std::string header()
+    {
+        return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << temperatureTimestamp << "," << temperature << "\n";
+    }
+};
+
+struct LPS28DFW_2Data : Boardcore::LPS28DFWData
+{
+    explicit LPS28DFW_2Data(const Boardcore::LPS28DFWData& data)
+        : Boardcore::LPS28DFWData(data)
+    {
+    }
+
+    LPS28DFW_2Data() {}
+
+    static std::string header()
+    {
+        return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << temperatureTimestamp << "," << temperature << "\n";
+    }
+};
+
+struct HSCMRNN015PA_1Data : Boardcore::HSCMRNN015PAData
+{
+    explicit HSCMRNN015PA_1Data(const Boardcore::HSCMRNN015PAData& data)
+        : Boardcore::HSCMRNN015PAData(data)
+    {
+    }
+
+    HSCMRNN015PA_1Data() {}
+
+    static std::string header() { return "pressureTimestamp,pressure\n "; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << "\n";
+    }
+};
+
+struct HSCMRNN015PA_2Data : Boardcore::HSCMRNN015PAData
+{
+    explicit HSCMRNN015PA_2Data(const Boardcore::HSCMRNN015PAData& data)
+        : Boardcore::HSCMRNN015PAData(data)
+    {
+    }
+
+    HSCMRNN015PA_2Data() {}
+
+    static std::string header() { return "pressureTimestamp,pressure\n "; }
+
+    void print(std::ostream& os) const
+    {
+        os << pressureTimestamp << "," << pressure << ","
+           << "\n";
+    }
+};
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.cpp b/src/boards/Main/StateMachines/ABKController/ABKController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06f9c0d3cac0e3f502e5053a3ea6760d481e4783
--- /dev/null
+++ b/src/boards/Main/StateMachines/ABKController/ABKController.cpp
@@ -0,0 +1,219 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/Configs/ABKConfig.h>
+#include <Main/StateMachines/ABKController/ABKController.h>
+#include <Main/StateMachines/ABKController/TrajectorySet.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Main::ABKTrajectories;
+using namespace Common;
+
+namespace Main
+{
+
+ABKController::ABKController(TaskScheduler* sched)
+    : FSM(&ABKController::state_init),
+      abk(
+          []()
+          {
+              return TimedTrajectoryPoint(ModuleManager::getInstance()
+                                              .get<NASController>()
+                                              ->getNasState());
+          },
+          OPEN_TRAJECTORY_SET, CLOSED_TRAJECTORY_SET, ABKConfig::ABK_CONFIG,
+          getConfig(),
+          [](float position)
+          {
+              ModuleManager::getInstance().get<Actuators>()->setServoPosition(
+                  ServosList::AIR_BRAKES_SERVO, position);
+          }),
+      scheduler(sched)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_ABK);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+bool ABKController::start()
+{
+    // Insert the update task into the task scheduler
+    size_t result =
+        scheduler->addTask([&]() { update(); }, ABKConfig::UPDATE_PERIOD,
+                           TaskScheduler::Policy::RECOVER);
+
+    return result != 0 && ActiveObject::start();
+}
+
+void ABKController::update()
+{
+    // No need for pause kernel due to its presence inside the getter
+    ABKControllerStatus status = getStatus();
+
+    if (!abk.isRunning() && status.state == ABKControllerState::ACTIVE)
+    {
+        // Begin the algorithm with the last estimated mass
+        abk.begin(ModuleManager::getInstance()
+                      .get<MEAController>()
+                      ->getStatus()
+                      .estimatedMass);
+    }
+
+    // Update the algorithm if in Active mode
+    if (status.state == ABKControllerState::ACTIVE)
+    {
+        abk.update();
+    }
+}
+
+ABKControllerStatus ABKController::getStatus()
+{
+    miosix::PauseKernelLock lock;
+    return status;
+}
+
+void ABKController::state_init(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(ABKControllerState::INIT);
+
+            // Reset the servo position
+            modules.get<Actuators>()->setServoPosition(
+                ServosList::AIR_BRAKES_SERVO, 0);
+
+            return transition(&ABKController::state_idle);
+        }
+    }
+}
+
+void ABKController::state_idle(const Event& event)
+{
+    static uint16_t delayTimeoutEventId = 0;
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ABKControllerState::IDLE);
+        }
+        case FLIGHT_MOTOR_SHUTDOWN:
+        {
+            // Wait a fixed time to start the ABK algorithm, in order to let the
+            // MEA algorithm estimate correctly the mass
+            delayTimeoutEventId = EventBroker::getInstance().postDelayed(
+                ABK_SHADOW_MODE_TIMEOUT, TOPIC_ABK, ABKConfig::DELAY_TIMEOUT);
+            break;
+        }
+        case ABK_SHADOW_MODE_TIMEOUT:
+        {
+            return transition(&ABKController::state_active);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&ABKController::state_end);
+        }
+        case EV_EXIT:
+        {
+            // Remove the shadow mode event. This works even though the event is
+            // expired (aka after shadow_mode_timeout) because the event broker
+            // assigns a progressive number for every delayed event. If and only
+            // if the number of registered delayed event is less than 2^16, then
+            // this technique is valid.
+            return EventBroker::getInstance().removeDelayed(
+                delayTimeoutEventId);
+        }
+    }
+}
+
+void ABKController::state_active(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ABKControllerState::ACTIVE);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        case FLIGHT_APOGEE_DETECTED:
+        {
+            return transition(&ABKController::state_end);
+        }
+    }
+}
+
+void ABKController::state_end(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(ABKControllerState::END);
+            abk.end();
+
+            // Close the servo
+            modules.get<Actuators>()->setServoPosition(
+                ServosList::AIR_BRAKES_SERVO, 0);
+
+            return;
+        }
+    }
+}
+
+void ABKController::logStatus(ABKControllerState state)
+{
+    {
+        miosix::PauseKernelLock lock;
+        status.timestamp = TimestampTimer::getTimestamp();
+        status.state     = state;
+    }
+
+    Logger::getInstance().log(status);
+}
+
+AirBrakesInterpConfig ABKController::getConfig()
+{
+    AirBrakesInterpConfig config;
+
+    config.ABK_CRITICAL_ALTITUDE   = ABKConfig::ABK_CRITICAL_ALTITUDE;
+    config.STARTING_FILTER_VALUE   = ABKConfig::STARTING_FILTER_VALUE;
+    config.FILTER_MAXIMUM_ALTITUDE = ABKConfig::MAXIMUM_ALTITUDE;
+    config.FILTER_MINIMUM_ALTITUDE = ABKConfig::MINIMUM_ALTITUDE;
+    config.INITIAL_MASS            = ABKConfig::INITIAL_MASS;
+    config.N_FORWARD               = ABKConfig::N_FORWARD;
+    config.DM                      = ABKConfig::DM;
+    config.DZ                      = ABKConfig::DZ;
+
+    return config;
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.h b/src/boards/Main/StateMachines/ABKController/ABKController.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1c829578622e7d113eb3e9d88d02d2f792bc692
--- /dev/null
+++ b/src/boards/Main/StateMachines/ABKController/ABKController.h
@@ -0,0 +1,80 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/StateMachines/ABKController/ABKControllerData.h>
+#include <algorithms/AirBrakes/AirBrakesInterp.h>
+#include <events/FSM.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class ABKController : public Boardcore::Module,
+                      public Boardcore::FSM<ABKController>
+{
+public:
+    ABKController(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the update task and the FSM
+     */
+    bool start() override;
+
+    /**
+     * @brief Periodic called by the Task scheduler, updates the ABKInterp
+     * algorithm
+     */
+    void update();
+
+    /**
+     * @brief Returns the ABK FSM status along with the last update timestamp
+     */
+    ABKControllerStatus getStatus();
+
+    // FSM states
+    void state_init(const Boardcore::Event& event);
+    void state_idle(const Boardcore::Event& event);
+    void state_active(const Boardcore::Event& event);
+    void state_end(const Boardcore::Event& event);
+
+private:
+    /**
+     * @brief Updates the internal status and logs it
+     */
+    void logStatus(ABKControllerState state);
+
+    /**
+     * @brief Returns the default config applied to the ABK algorithm
+     */
+    Boardcore::AirBrakesInterpConfig getConfig();
+
+    Boardcore::AirBrakesInterp abk;
+
+    ABKControllerStatus status;
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ABK");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ABKController/ABKControllerData.h b/src/boards/Main/StateMachines/ABKController/ABKControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..eff84f995e33b2e8e9aa07eeac613bce18eaff73
--- /dev/null
+++ b/src/boards/Main/StateMachines/ABKController/ABKControllerData.h
@@ -0,0 +1,52 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+
+namespace Main
+{
+enum class ABKControllerState : uint8_t
+{
+    UNINIT = 0,
+    INIT,
+    IDLE,
+    ACTIVE,
+    END
+};
+
+struct ABKControllerStatus
+{
+    uint64_t timestamp       = 0;
+    ABKControllerState state = ABKControllerState::UNINIT;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ABKController/TrajectorySet.h b/src/boards/Main/StateMachines/ABKController/TrajectorySet.h
new file mode 100644
index 0000000000000000000000000000000000000000..f46a3e1857aaa725100074530a4a2e3c305a1d7a
--- /dev/null
+++ b/src/boards/Main/StateMachines/ABKController/TrajectorySet.h
@@ -0,0 +1,2443 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/AirBrakes/TrajectorySet.h>
+
+namespace Main
+{
+namespace ABKTrajectories
+{
+//['Heights ', 'Vz_closed_m26 ', 'Vz_closed_m26_1 ', 'Vz_closed_m26_2 ',
+//'Vz_closed_m26_3 ', 'Vz_closed_m26_4 ', 'Vz_closed_m26_5 ', 'Vz_closed_m26_6
+//', 'Vz_closed_m26_7 ', 'Vz_closed_m26_8 ', 'Vz_closed_m26_9 ', 'Vz_closed_m27
+//', 'Vz_open_m26 ', 'Vz_open_m26_1 ', 'Vz_open_m26_2 ', 'Vz_open_m26_3 ',
+//'Vz_open_m26_4 ', 'Vz_open_m26_5 ', 'Vz_open_m26_6 ', 'Vz_open_m26_7 ',
+//'Vz_open_m26_8 ', 'Vz_open_m26_9 ', 'Vz_open_m27 ']
+Boardcore::TrajectoryPoint t0_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.281183589541),
+    Boardcore::TrajectoryPoint(10, 151.46876639657),
+    Boardcore::TrajectoryPoint(20, 150.654145389855),
+    Boardcore::TrajectoryPoint(30, 149.836780149099),
+    Boardcore::TrajectoryPoint(40, 149.016705742141),
+    Boardcore::TrajectoryPoint(50, 148.194090806251),
+    Boardcore::TrajectoryPoint(60, 147.368545299231),
+    Boardcore::TrajectoryPoint(70, 146.540184456041),
+    Boardcore::TrajectoryPoint(80, 145.709156237422),
+    Boardcore::TrajectoryPoint(90, 144.874895029495),
+    Boardcore::TrajectoryPoint(100, 144.037811823631),
+    Boardcore::TrajectoryPoint(110, 143.197745207749),
+    Boardcore::TrajectoryPoint(120, 142.354379620129),
+    Boardcore::TrajectoryPoint(130, 141.508113667175),
+    Boardcore::TrajectoryPoint(140, 140.65835031699),
+    Boardcore::TrajectoryPoint(150, 139.805461647332),
+    Boardcore::TrajectoryPoint(160, 138.949190516325),
+    Boardcore::TrajectoryPoint(170, 138.089427939597),
+    Boardcore::TrajectoryPoint(180, 137.226341439041),
+    Boardcore::TrajectoryPoint(190, 136.359442928428),
+    Boardcore::TrajectoryPoint(200, 135.489217534117),
+    Boardcore::TrajectoryPoint(210, 134.614909634047),
+    Boardcore::TrajectoryPoint(220, 133.737204982698),
+    Boardcore::TrajectoryPoint(230, 132.855202173581),
+    Boardcore::TrajectoryPoint(240, 131.969660088597),
+    Boardcore::TrajectoryPoint(250, 131.079664105217),
+    Boardcore::TrajectoryPoint(260, 130.185907545998),
+    Boardcore::TrajectoryPoint(270, 129.287606645828),
+    Boardcore::TrajectoryPoint(280, 128.385238574269),
+    Boardcore::TrajectoryPoint(290, 127.478306751639),
+    Boardcore::TrajectoryPoint(300, 126.566908908799),
+    Boardcore::TrajectoryPoint(310, 125.651005050447),
+    Boardcore::TrajectoryPoint(320, 124.730136635695),
+    Boardcore::TrajectoryPoint(330, 123.804728493886),
+    Boardcore::TrajectoryPoint(340, 122.874099856926),
+    Boardcore::TrajectoryPoint(350, 121.938494092651),
+    Boardcore::TrajectoryPoint(360, 120.997934171153),
+    Boardcore::TrajectoryPoint(370, 120.051624825784),
+    Boardcore::TrajectoryPoint(380, 119.100084871254),
+    Boardcore::TrajectoryPoint(390, 118.14315807336),
+    Boardcore::TrajectoryPoint(400, 117.180100240481),
+    Boardcore::TrajectoryPoint(410, 116.211315398257),
+    Boardcore::TrajectoryPoint(420, 115.236655226956),
+    Boardcore::TrajectoryPoint(430, 114.255726725382),
+    Boardcore::TrajectoryPoint(440, 113.268259505295),
+    Boardcore::TrajectoryPoint(450, 112.274378373159),
+    Boardcore::TrajectoryPoint(460, 111.273884006837),
+    Boardcore::TrajectoryPoint(470, 110.266569375787),
+    Boardcore::TrajectoryPoint(480, 109.251883070934),
+    Boardcore::TrajectoryPoint(490, 108.229915553916),
+    Boardcore::TrajectoryPoint(500, 107.200487643897),
+    Boardcore::TrajectoryPoint(510, 106.163359979054),
+    Boardcore::TrajectoryPoint(520, 105.118283626543),
+    Boardcore::TrajectoryPoint(530, 104.064999619219),
+    Boardcore::TrajectoryPoint(540, 103.003238464434),
+    Boardcore::TrajectoryPoint(550, 101.932719622999),
+    Boardcore::TrajectoryPoint(560, 100.853150956205),
+    Boardcore::TrajectoryPoint(570, 99.7642281386292),
+    Boardcore::TrajectoryPoint(580, 98.665634034297),
+    Boardcore::TrajectoryPoint(590, 97.5570380335149),
+    Boardcore::TrajectoryPoint(600, 96.4380953475082),
+    Boardcore::TrajectoryPoint(610, 95.3084462577238),
+    Boardcore::TrajectoryPoint(620, 94.1677153163993),
+    Boardcore::TrajectoryPoint(630, 93.0155104946976),
+    Boardcore::TrajectoryPoint(640, 91.8514222743819),
+    Boardcore::TrajectoryPoint(650, 90.6750226786436),
+    Boardcore::TrajectoryPoint(660, 89.4856019474907),
+    Boardcore::TrajectoryPoint(670, 88.282533767652),
+    Boardcore::TrajectoryPoint(680, 87.0656475005727),
+    Boardcore::TrajectoryPoint(690, 85.8344235136038),
+    Boardcore::TrajectoryPoint(700, 84.5874947465647),
+    Boardcore::TrajectoryPoint(710, 83.324827217275),
+    Boardcore::TrajectoryPoint(720, 82.0457096278374),
+    Boardcore::TrajectoryPoint(730, 80.7486749628618),
+    Boardcore::TrajectoryPoint(740, 79.4338607615884),
+    Boardcore::TrajectoryPoint(750, 78.0991665033548),
+    Boardcore::TrajectoryPoint(760, 76.7446619169102),
+    Boardcore::TrajectoryPoint(770, 75.3685866225035),
+    Boardcore::TrajectoryPoint(780, 73.9698278034401),
+    Boardcore::TrajectoryPoint(790, 72.5474663089349),
+    Boardcore::TrajectoryPoint(800, 71.0998994747218),
+    Boardcore::TrajectoryPoint(810, 69.6249411642998),
+    Boardcore::TrajectoryPoint(820, 68.1214404232699),
+    Boardcore::TrajectoryPoint(830, 66.5873975730555),
+    Boardcore::TrajectoryPoint(840, 65.0206274307197),
+    Boardcore::TrajectoryPoint(850, 63.4187375411119),
+    Boardcore::TrajectoryPoint(860, 61.7788913095777),
+    Boardcore::TrajectoryPoint(870, 60.0974916035063),
+    Boardcore::TrajectoryPoint(880, 58.3718398588616),
+    Boardcore::TrajectoryPoint(890, 56.597541226983),
+    Boardcore::TrajectoryPoint(900, 54.7696610514191),
+    Boardcore::TrajectoryPoint(910, 52.8826329147933),
+    Boardcore::TrajectoryPoint(920, 50.9302854325946),
+    Boardcore::TrajectoryPoint(930, 48.9044877152189),
+    Boardcore::TrajectoryPoint(940, 46.7957255802411),
+    Boardcore::TrajectoryPoint(950, 44.592796083963),
+    Boardcore::TrajectoryPoint(960, 42.2794707199437),
+    Boardcore::TrajectoryPoint(970, 39.8381425902815),
+    Boardcore::TrajectoryPoint(980, 37.2430292543444),
+    Boardcore::TrajectoryPoint(990, 34.4599032639387),
+    Boardcore::TrajectoryPoint(1000, 31.4379167461659),
+    Boardcore::TrajectoryPoint(1010, 28.1016053399742),
+    Boardcore::TrajectoryPoint(1020, 24.3197095473458),
+    Boardcore::TrajectoryPoint(1030, 19.8426201220378),
+    Boardcore::TrajectoryPoint(1040, 14.0185018340729),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t1_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.245755350089),
+    Boardcore::TrajectoryPoint(10, 151.433869072696),
+    Boardcore::TrajectoryPoint(20, 150.619758851732),
+    Boardcore::TrajectoryPoint(30, 149.802933721734),
+    Boardcore::TrajectoryPoint(40, 148.983362845367),
+    Boardcore::TrajectoryPoint(50, 148.161248211031),
+    Boardcore::TrajectoryPoint(60, 147.336231679034),
+    Boardcore::TrajectoryPoint(70, 146.508363917621),
+    Boardcore::TrajectoryPoint(80, 145.677825579755),
+    Boardcore::TrajectoryPoint(90, 144.844082149046),
+    Boardcore::TrajectoryPoint(100, 144.007481641361),
+    Boardcore::TrajectoryPoint(110, 143.167925542373),
+    Boardcore::TrajectoryPoint(120, 142.325035490906),
+    Boardcore::TrajectoryPoint(130, 141.479241919132),
+    Boardcore::TrajectoryPoint(140, 140.629977782493),
+    Boardcore::TrajectoryPoint(150, 139.777554371874),
+    Boardcore::TrajectoryPoint(160, 138.921775088473),
+    Boardcore::TrajectoryPoint(170, 138.062470671407),
+    Boardcore::TrajectoryPoint(180, 137.199868605701),
+    Boardcore::TrajectoryPoint(190, 136.333421179004),
+    Boardcore::TrajectoryPoint(200, 135.463672764336),
+    Boardcore::TrajectoryPoint(210, 134.589808896816),
+    Boardcore::TrajectoryPoint(220, 133.712554201462),
+    Boardcore::TrajectoryPoint(230, 132.831007928586),
+    Boardcore::TrajectoryPoint(240, 131.945927801359),
+    Boardcore::TrajectoryPoint(250, 131.056361823404),
+    Boardcore::TrajectoryPoint(260, 130.163059662486),
+    Boardcore::TrajectoryPoint(270, 129.265181792979),
+    Boardcore::TrajectoryPoint(280, 128.363260532298),
+    Boardcore::TrajectoryPoint(290, 127.45674479197),
+    Boardcore::TrajectoryPoint(300, 126.5457861476),
+    Boardcore::TrajectoryPoint(310, 125.630291449902),
+    Boardcore::TrajectoryPoint(320, 124.709854598997),
+    Boardcore::TrajectoryPoint(330, 123.784874274836),
+    Boardcore::TrajectoryPoint(340, 122.854643995785),
+    Boardcore::TrajectoryPoint(350, 121.919458367271),
+    Boardcore::TrajectoryPoint(360, 120.97928994655),
+    Boardcore::TrajectoryPoint(370, 120.033393039748),
+    Boardcore::TrajectoryPoint(380, 119.0822616981),
+    Boardcore::TrajectoryPoint(390, 118.125739662359),
+    Boardcore::TrajectoryPoint(400, 117.163058733959),
+    Boardcore::TrajectoryPoint(410, 116.194670899962),
+    Boardcore::TrajectoryPoint(420, 115.22040384241),
+    Boardcore::TrajectoryPoint(430, 114.239841598892),
+    Boardcore::TrajectoryPoint(440, 113.252759712339),
+    Boardcore::TrajectoryPoint(450, 112.259259980483),
+    Boardcore::TrajectoryPoint(460, 111.259143061607),
+    Boardcore::TrajectoryPoint(470, 110.252201906462),
+    Boardcore::TrajectoryPoint(480, 109.237863577153),
+    Boardcore::TrajectoryPoint(490, 108.216261706941),
+    Boardcore::TrajectoryPoint(500, 107.18719544172),
+    Boardcore::TrajectoryPoint(510, 106.150425403584),
+    Boardcore::TrajectoryPoint(520, 105.105702644359),
+    Boardcore::TrajectoryPoint(530, 104.052768182294),
+    Boardcore::TrajectoryPoint(540, 102.991352510841),
+    Boardcore::TrajectoryPoint(550, 101.921175077596),
+    Boardcore::TrajectoryPoint(560, 100.841943731293),
+    Boardcore::TrajectoryPoint(570, 99.7533541346046),
+    Boardcore::TrajectoryPoint(580, 98.6550891402744),
+    Boardcore::TrajectoryPoint(590, 97.5468181279407),
+    Boardcore::TrajectoryPoint(600, 96.4281962987566),
+    Boardcore::TrajectoryPoint(610, 95.298863924679),
+    Boardcore::TrajectoryPoint(620, 94.1584455490239),
+    Boardcore::TrajectoryPoint(630, 93.006549134589),
+    Boardcore::TrajectoryPoint(640, 91.842765155316),
+    Boardcore::TrajectoryPoint(650, 90.6666656271074),
+    Boardcore::TrajectoryPoint(660, 89.4775565308792),
+    Boardcore::TrajectoryPoint(670, 88.2747795855256),
+    Boardcore::TrajectoryPoint(680, 87.0581803478825),
+    Boardcore::TrajectoryPoint(690, 85.8272391803842),
+    Boardcore::TrajectoryPoint(700, 84.5806033657714),
+    Boardcore::TrajectoryPoint(710, 83.3182096973134),
+    Boardcore::TrajectoryPoint(720, 82.0393755312979),
+    Boardcore::TrajectoryPoint(730, 80.7426057000813),
+    Boardcore::TrajectoryPoint(740, 79.4280652371814),
+    Boardcore::TrajectoryPoint(750, 78.0936267256959),
+    Boardcore::TrajectoryPoint(760, 76.7393860502793),
+    Boardcore::TrajectoryPoint(770, 75.3635573637401),
+    Boardcore::TrajectoryPoint(780, 73.9650524991158),
+    Boardcore::TrajectoryPoint(790, 72.5429398221279),
+    Boardcore::TrajectoryPoint(800, 71.095605476199),
+    Boardcore::TrajectoryPoint(810, 69.6208858323125),
+    Boardcore::TrajectoryPoint(820, 68.1176184924483),
+    Boardcore::TrajectoryPoint(830, 66.583803729791),
+    Boardcore::TrajectoryPoint(840, 65.0172563157423),
+    Boardcore::TrajectoryPoint(850, 63.4155837519961),
+    Boardcore::TrajectoryPoint(860, 61.7759584045937),
+    Boardcore::TrajectoryPoint(870, 60.0947646527577),
+    Boardcore::TrajectoryPoint(880, 58.3693133212936),
+    Boardcore::TrajectoryPoint(890, 56.595217401955),
+    Boardcore::TrajectoryPoint(900, 54.7675259349765),
+    Boardcore::TrajectoryPoint(910, 52.8806879096757),
+    Boardcore::TrajectoryPoint(920, 50.9285239468375),
+    Boardcore::TrajectoryPoint(930, 48.9029030431781),
+    Boardcore::TrajectoryPoint(940, 46.794310910424),
+    Boardcore::TrajectoryPoint(950, 44.5915445070942),
+    Boardcore::TrajectoryPoint(960, 42.2783803832867),
+    Boardcore::TrajectoryPoint(970, 39.8372054912072),
+    Boardcore::TrajectoryPoint(980, 37.2422371834903),
+    Boardcore::TrajectoryPoint(990, 34.4592517906086),
+    Boardcore::TrajectoryPoint(1000, 31.4373963163521),
+    Boardcore::TrajectoryPoint(1010, 28.1012061102367),
+    Boardcore::TrajectoryPoint(1020, 24.3194264604742),
+    Boardcore::TrajectoryPoint(1030, 19.842442589616),
+    Boardcore::TrajectoryPoint(1040, 14.0184197802997),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t2_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.210609519524),
+    Boardcore::TrajectoryPoint(10, 151.399251019435),
+    Boardcore::TrajectoryPoint(20, 150.585647363659),
+    Boardcore::TrajectoryPoint(30, 149.769357969649),
+    Boardcore::TrajectoryPoint(40, 148.950286467789),
+    Boardcore::TrajectoryPoint(50, 148.128668008159),
+    Boardcore::TrajectoryPoint(60, 147.304176169823),
+    Boardcore::TrajectoryPoint(70, 146.476797427427),
+    Boardcore::TrajectoryPoint(80, 145.646744936456),
+    Boardcore::TrajectoryPoint(90, 144.813515095859),
+    Boardcore::TrajectoryPoint(100, 143.977393316162),
+    Boardcore::TrajectoryPoint(110, 143.138343607042),
+    Boardcore::TrajectoryPoint(120, 142.295925184969),
+    Boardcore::TrajectoryPoint(130, 141.450600115973),
+    Boardcore::TrajectoryPoint(140, 140.601831161473),
+    Boardcore::TrajectoryPoint(150, 139.74986919445),
+    Boardcore::TrajectoryPoint(160, 138.894577788639),
+    Boardcore::TrajectoryPoint(170, 138.035727778478),
+    Boardcore::TrajectoryPoint(180, 137.173606239555),
+    Boardcore::TrajectoryPoint(190, 136.307606206417),
+    Boardcore::TrajectoryPoint(200, 135.438330589448),
+    Boardcore::TrajectoryPoint(210, 134.56490746254),
+    Boardcore::TrajectoryPoint(220, 133.688090125555),
+    Boardcore::TrajectoryPoint(230, 132.807005637333),
+    Boardcore::TrajectoryPoint(240, 131.922356150717),
+    Boardcore::TrajectoryPoint(250, 131.033244270852),
+    Boardcore::TrajectoryPoint(260, 130.140392852358),
+    Boardcore::TrajectoryPoint(270, 129.242934569645),
+    Boardcore::TrajectoryPoint(280, 128.341456527848),
+    Boardcore::TrajectoryPoint(290, 127.435353486752),
+    Boardcore::TrajectoryPoint(300, 126.524830512989),
+    Boardcore::TrajectoryPoint(310, 125.609741653331),
+    Boardcore::TrajectoryPoint(320, 124.689732902745),
+    Boardcore::TrajectoryPoint(330, 123.765176963417),
+    Boardcore::TrajectoryPoint(340, 122.835341813559),
+    Boardcore::TrajectoryPoint(350, 121.900572952773),
+    Boardcore::TrajectoryPoint(360, 120.960792863729),
+    Boardcore::TrajectoryPoint(370, 120.015305092351),
+    Boardcore::TrajectoryPoint(380, 119.064579091961),
+    Boardcore::TrajectoryPoint(390, 118.108435543303),
+    Boardcore::TrajectoryPoint(400, 117.146151511612),
+    Boardcore::TrajectoryPoint(410, 116.178157511366),
+    Boardcore::TrajectoryPoint(420, 115.204280425278),
+    Boardcore::TrajectoryPoint(430, 114.224081488583),
+    Boardcore::TrajectoryPoint(440, 113.237381858813),
+    Boardcore::TrajectoryPoint(450, 112.244260483028),
+    Boardcore::TrajectoryPoint(460, 111.244518000119),
+    Boardcore::TrajectoryPoint(470, 110.237947342303),
+    Boardcore::TrajectoryPoint(480, 109.223954194018),
+    Boardcore::TrajectoryPoint(490, 108.202715057859),
+    Boardcore::TrajectoryPoint(500, 107.174007557828),
+    Boardcore::TrajectoryPoint(510, 106.137592300079),
+    Boardcore::TrajectoryPoint(520, 105.093220321237),
+    Boardcore::TrajectoryPoint(530, 104.040632625069),
+    Boardcore::TrajectoryPoint(540, 102.97955969124),
+    Boardcore::TrajectoryPoint(550, 101.909720954233),
+    Boardcore::TrajectoryPoint(560, 100.830824250328),
+    Boardcore::TrajectoryPoint(570, 99.7425652303751),
+    Boardcore::TrajectoryPoint(580, 98.6446267359209),
+    Boardcore::TrajectoryPoint(590, 97.5366781360087),
+    Boardcore::TrajectoryPoint(600, 96.4183746217864),
+    Boardcore::TrajectoryPoint(610, 95.2893564557807),
+    Boardcore::TrajectoryPoint(620, 94.1492481724396),
+    Boardcore::TrajectoryPoint(630, 92.9976577262422),
+    Boardcore::TrajectoryPoint(640, 91.8341755833496),
+    Boardcore::TrajectoryPoint(650, 90.65837375241),
+    Boardcore::TrajectoryPoint(660, 89.4695738404114),
+    Boardcore::TrajectoryPoint(670, 88.2670858315855),
+    Boardcore::TrajectoryPoint(680, 87.0507713600094),
+    Boardcore::TrajectoryPoint(690, 85.8201107832082),
+    Boardcore::TrajectoryPoint(700, 84.5737656215671),
+    Boardcore::TrajectoryPoint(710, 83.3116436583328),
+    Boardcore::TrajectoryPoint(720, 82.0330906930786),
+    Boardcore::TrajectoryPoint(730, 80.7365836136397),
+    Boardcore::TrajectoryPoint(740, 79.4223147445155),
+    Boardcore::TrajectoryPoint(750, 78.0881299717913),
+    Boardcore::TrajectoryPoint(760, 76.7341511419843),
+    Boardcore::TrajectoryPoint(770, 75.3585671296012),
+    Boardcore::TrajectoryPoint(780, 73.9603142341499),
+    Boardcore::TrajectoryPoint(790, 72.5384484305309),
+    Boardcore::TrajectoryPoint(800, 71.091344753525),
+    Boardcore::TrajectoryPoint(810, 69.6168619135995),
+    Boardcore::TrajectoryPoint(820, 68.1138261544216),
+    Boardcore::TrajectoryPoint(830, 66.580237701304),
+    Boardcore::TrajectoryPoint(840, 65.0139112803265),
+    Boardcore::TrajectoryPoint(850, 63.4124543503412),
+    Boardcore::TrajectoryPoint(860, 61.7730481705832),
+    Boardcore::TrajectoryPoint(870, 60.0920587713138),
+    Boardcore::TrajectoryPoint(880, 58.3668062954703),
+    Boardcore::TrajectoryPoint(890, 56.5929115159511),
+    Boardcore::TrajectoryPoint(900, 54.7654072928502),
+    Boardcore::TrajectoryPoint(910, 52.8787579056344),
+    Boardcore::TrajectoryPoint(920, 50.926776040805),
+    Boardcore::TrajectoryPoint(930, 48.9013305822642),
+    Boardcore::TrajectoryPoint(940, 46.7929071366803),
+    Boardcore::TrajectoryPoint(950, 44.5903025655055),
+    Boardcore::TrajectoryPoint(960, 42.2772984368827),
+    Boardcore::TrajectoryPoint(970, 39.8362755998336),
+    Boardcore::TrajectoryPoint(980, 37.2414512018354),
+    Boardcore::TrajectoryPoint(990, 34.4586053231666),
+    Boardcore::TrajectoryPoint(1000, 31.4368798833573),
+    Boardcore::TrajectoryPoint(1010, 28.100809944689),
+    Boardcore::TrajectoryPoint(1020, 24.3191455450002),
+    Boardcore::TrajectoryPoint(1030, 19.8422664178905),
+    Boardcore::TrajectoryPoint(1040, 14.0183383547549),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t3_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.175742743635),
+    Boardcore::TrajectoryPoint(10, 151.364908902743),
+    Boardcore::TrajectoryPoint(20, 150.551807643526),
+    Boardcore::TrajectoryPoint(30, 149.736049663359),
+    Boardcore::TrajectoryPoint(40, 148.917473431018),
+    Boardcore::TrajectoryPoint(50, 148.096347069949),
+    Boardcore::TrajectoryPoint(60, 147.272375695397),
+    Boardcore::TrajectoryPoint(70, 146.445481959129),
+    Boardcore::TrajectoryPoint(80, 145.615911330684),
+    Boardcore::TrajectoryPoint(90, 144.783190943419),
+    Boardcore::TrajectoryPoint(100, 143.947543970178),
+    Boardcore::TrajectoryPoint(110, 143.108996573506),
+    Boardcore::TrajectoryPoint(120, 142.267045921899),
+    Boardcore::TrajectoryPoint(130, 141.422185524735),
+    Boardcore::TrajectoryPoint(140, 140.573907769392),
+    Boardcore::TrajectoryPoint(150, 139.722403477161),
+    Boardcore::TrajectoryPoint(160, 138.867596026598),
+    Boardcore::TrajectoryPoint(170, 138.00919671641),
+    Boardcore::TrajectoryPoint(180, 137.147551843121),
+    Boardcore::TrajectoryPoint(190, 136.281995558196),
+    Boardcore::TrajectoryPoint(200, 135.413160653025),
+    Boardcore::TrajectoryPoint(210, 134.540202969109),
+    Boardcore::TrajectoryPoint(220, 133.66381943345),
+    Boardcore::TrajectoryPoint(230, 132.783193026494),
+    Boardcore::TrajectoryPoint(240, 131.898970455045),
+    Boardcore::TrajectoryPoint(250, 131.010309261439),
+    Boardcore::TrajectoryPoint(260, 130.117904973303),
+    Boardcore::TrajectoryPoint(270, 129.22086287533),
+    Boardcore::TrajectoryPoint(280, 128.319824503446),
+    Boardcore::TrajectoryPoint(290, 127.414130819542),
+    Boardcore::TrajectoryPoint(300, 126.504040030746),
+    Boardcore::TrajectoryPoint(310, 125.589353726765),
+    Boardcore::TrajectoryPoint(320, 124.669769654394),
+    Boardcore::TrajectoryPoint(330, 123.745634708138),
+    Boardcore::TrajectoryPoint(340, 122.816191497802),
+    Boardcore::TrajectoryPoint(350, 121.88183607696),
+    Boardcore::TrajectoryPoint(360, 120.942441188767),
+    Boardcore::TrajectoryPoint(370, 119.997359289112),
+    Boardcore::TrajectoryPoint(380, 119.047035397413),
+    Boardcore::TrajectoryPoint(390, 118.091266519636),
+    Boardcore::TrajectoryPoint(400, 117.12937699335),
+    Boardcore::TrajectoryPoint(410, 116.161773690232),
+    Boardcore::TrajectoryPoint(420, 115.188283470785),
+    Boardcore::TrajectoryPoint(430, 114.208444925174),
+    Boardcore::TrajectoryPoint(440, 113.222124512079),
+    Boardcore::TrajectoryPoint(450, 112.229378484405),
+    Boardcore::TrajectoryPoint(460, 111.230007461829),
+    Boardcore::TrajectoryPoint(470, 110.223790888272),
+    Boardcore::TrajectoryPoint(480, 109.210153629929),
+    Boardcore::TrajectoryPoint(490, 108.189274349688),
+    Boardcore::TrajectoryPoint(500, 107.160922769454),
+    Boardcore::TrajectoryPoint(510, 106.124859479576),
+    Boardcore::TrajectoryPoint(520, 105.080835501611),
+    Boardcore::TrajectoryPoint(530, 104.02859182496),
+    Boardcore::TrajectoryPoint(540, 102.967858915615),
+    Boardcore::TrajectoryPoint(550, 101.89835619505),
+    Boardcore::TrajectoryPoint(560, 100.819791487184),
+    Boardcore::TrajectoryPoint(570, 99.7318604311362),
+    Boardcore::TrajectoryPoint(580, 98.6342458573339),
+    Boardcore::TrajectoryPoint(590, 97.5266171242993),
+    Boardcore::TrajectoryPoint(600, 96.4086294132409),
+    Boardcore::TrajectoryPoint(610, 95.279922977315),
+    Boardcore::TrajectoryPoint(620, 94.1401223421543),
+    Boardcore::TrajectoryPoint(630, 92.9888354539658),
+    Boardcore::TrajectoryPoint(640, 91.8256527711702),
+    Boardcore::TrajectoryPoint(650, 90.6501462951963),
+    Boardcore::TrajectoryPoint(660, 89.4616531455046),
+    Boardcore::TrajectoryPoint(670, 88.2594518023261),
+    Boardcore::TrajectoryPoint(680, 87.043419860101),
+    Boardcore::TrajectoryPoint(690, 85.813037671454),
+    Boardcore::TrajectoryPoint(700, 84.5669808902813),
+    Boardcore::TrajectoryPoint(710, 83.3051285020036),
+    Boardcore::TrajectoryPoint(720, 82.0268545408799),
+    Boardcore::TrajectoryPoint(730, 80.730608155684),
+    Boardcore::TrajectoryPoint(740, 79.4166087608309),
+    Boardcore::TrajectoryPoint(750, 78.0826757424302),
+    Boardcore::TrajectoryPoint(760, 76.7289567169571),
+    Boardcore::TrajectoryPoint(770, 75.3536154676685),
+    Boardcore::TrajectoryPoint(780, 73.955612579305),
+    Boardcore::TrajectoryPoint(790, 72.5339917275971),
+    Boardcore::TrajectoryPoint(800, 71.087116921422),
+    Boardcore::TrajectoryPoint(810, 69.6128690445966),
+    Boardcore::TrajectoryPoint(820, 68.1100630668372),
+    Boardcore::TrajectoryPoint(830, 66.5766991659472),
+    Boardcore::TrajectoryPoint(840, 65.0105920230206),
+    Boardcore::TrajectoryPoint(850, 63.4093490543778),
+    Boardcore::TrajectoryPoint(860, 61.7701603457035),
+    Boardcore::TrajectoryPoint(870, 60.0893737159412),
+    Boardcore::TrajectoryPoint(880, 58.3643185562435),
+    Boardcore::TrajectoryPoint(890, 56.5906233620528),
+    Boardcore::TrajectoryPoint(900, 54.7633049351075),
+    Boardcore::TrajectoryPoint(910, 52.8768427297941),
+    Boardcore::TrajectoryPoint(920, 50.9250415580696),
+    Boardcore::TrajectoryPoint(930, 48.8997701918773),
+    Boardcore::TrajectoryPoint(940, 46.7915141336095),
+    Boardcore::TrajectoryPoint(950, 44.5890701483595),
+    Boardcore::TrajectoryPoint(960, 42.2762247842582),
+    Boardcore::TrajectoryPoint(970, 39.8353528333233),
+    Boardcore::TrajectoryPoint(980, 37.2406712394317),
+    Boardcore::TrajectoryPoint(990, 34.457963804137),
+    Boardcore::TrajectoryPoint(1000, 31.436367401316),
+    Boardcore::TrajectoryPoint(1010, 28.1004168081889),
+    Boardcore::TrajectoryPoint(1020, 24.3188667760365),
+    Boardcore::TrajectoryPoint(1030, 19.8420915912781),
+    Boardcore::TrajectoryPoint(1040, 14.0182575502518),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t4_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.141151720961),
+    Boardcore::TrajectoryPoint(10, 151.330839441355),
+    Boardcore::TrajectoryPoint(20, 150.518236461159),
+    Boardcore::TrajectoryPoint(30, 149.703005624475),
+    Boardcore::TrajectoryPoint(40, 148.884920606928),
+    Boardcore::TrajectoryPoint(50, 148.064282318157),
+    Boardcore::TrajectoryPoint(60, 147.240827228178),
+    Boardcore::TrajectoryPoint(70, 146.414414534208),
+    Boardcore::TrajectoryPoint(80, 145.585321832603),
+    Boardcore::TrajectoryPoint(90, 144.753106811422),
+    Boardcore::TrajectoryPoint(100, 143.917930770972),
+    Boardcore::TrajectoryPoint(110, 143.079881658144),
+    Boardcore::TrajectoryPoint(120, 142.238394965134),
+    Boardcore::TrajectoryPoint(130, 141.393995455544),
+    Boardcore::TrajectoryPoint(140, 140.546204964029),
+    Boardcore::TrajectoryPoint(150, 139.695154623667),
+    Boardcore::TrajectoryPoint(160, 138.840827252932),
+    Boardcore::TrajectoryPoint(170, 137.982874980864),
+    Boardcore::TrajectoryPoint(180, 137.121702958234),
+    Boardcore::TrajectoryPoint(190, 136.256586820462),
+    Boardcore::TrajectoryPoint(200, 135.388189052897),
+    Boardcore::TrajectoryPoint(210, 134.515693091553),
+    Boardcore::TrajectoryPoint(220, 133.639739843317),
+    Boardcore::TrajectoryPoint(230, 132.759567858465),
+    Boardcore::TrajectoryPoint(240, 131.875768748267),
+    Boardcore::TrajectoryPoint(250, 130.987554643371),
+    Boardcore::TrajectoryPoint(260, 130.095593916644),
+    Boardcore::TrajectoryPoint(270, 129.198964642501),
+    Boardcore::TrajectoryPoint(280, 128.298362433894),
+    Boardcore::TrajectoryPoint(290, 127.393074805514),
+    Boardcore::TrajectoryPoint(300, 126.483412757601),
+    Boardcore::TrajectoryPoint(310, 125.569125766536),
+    Boardcore::TrajectoryPoint(320, 124.649962991044),
+    Boardcore::TrajectoryPoint(330, 123.726245686504),
+    Boardcore::TrajectoryPoint(340, 122.797191264436),
+    Boardcore::TrajectoryPoint(350, 121.863245995363),
+    Boardcore::TrajectoryPoint(360, 120.924233214861),
+    Boardcore::TrajectoryPoint(370, 119.979553962042),
+    Boardcore::TrajectoryPoint(380, 119.029628984907),
+    Boardcore::TrajectoryPoint(390, 118.074231775202),
+    Boardcore::TrajectoryPoint(400, 117.112733623761),
+    Boardcore::TrajectoryPoint(410, 116.145517918404),
+    Boardcore::TrajectoryPoint(420, 115.172411497645),
+    Boardcore::TrajectoryPoint(430, 114.192930462305),
+    Boardcore::TrajectoryPoint(440, 113.206986261844),
+    Boardcore::TrajectoryPoint(450, 112.214612609992),
+    Boardcore::TrajectoryPoint(460, 111.215610107394),
+    Boardcore::TrajectoryPoint(470, 110.209736972506),
+    Boardcore::TrajectoryPoint(480, 109.196460613393),
+    Boardcore::TrajectoryPoint(490, 108.175938345013),
+    Boardcore::TrajectoryPoint(500, 107.147939872857),
+    Boardcore::TrajectoryPoint(510, 106.112225771609),
+    Boardcore::TrajectoryPoint(520, 105.06854704788),
+    Boardcore::TrajectoryPoint(530, 104.016644676831),
+    Boardcore::TrajectoryPoint(540, 102.956249110886),
+    Boardcore::TrajectoryPoint(550, 101.887079758615),
+    Boardcore::TrajectoryPoint(560, 100.808844431669),
+    Boardcore::TrajectoryPoint(570, 99.7212387575233),
+    Boardcore::TrajectoryPoint(580, 98.6239455555655),
+    Boardcore::TrajectoryPoint(590, 97.5166341738686),
+    Boardcore::TrajectoryPoint(600, 96.3989597837677),
+    Boardcore::TrajectoryPoint(610, 95.2705626291071),
+    Boardcore::TrajectoryPoint(620, 94.1310672267567),
+    Boardcore::TrajectoryPoint(630, 92.9800815146977),
+    Boardcore::TrajectoryPoint(640, 91.8171959436505),
+    Boardcore::TrajectoryPoint(650, 90.6419825078582),
+    Boardcore::TrajectoryPoint(660, 89.4537937268754),
+    Boardcore::TrajectoryPoint(670, 88.2518768051171),
+    Boardcore::TrajectoryPoint(680, 87.0361251817641),
+    Boardcore::TrajectoryPoint(690, 85.806019204549),
+    Boardcore::TrajectoryPoint(700, 84.5602485578731),
+    Boardcore::TrajectoryPoint(710, 83.2986636392305),
+    Boardcore::TrajectoryPoint(720, 82.0206665112317),
+    Boardcore::TrajectoryPoint(730, 80.7246787868096),
+    Boardcore::TrajectoryPoint(740, 79.4109467714264),
+    Boardcore::TrajectoryPoint(750, 78.0772635460938),
+    Boardcore::TrajectoryPoint(760, 76.723802307447),
+    Boardcore::TrajectoryPoint(770, 75.3487019324884),
+    Boardcore::TrajectoryPoint(780, 73.9509471119494),
+    Boardcore::TrajectoryPoint(790, 72.5295693130338),
+    Boardcore::TrajectoryPoint(800, 71.0829216005362),
+    Boardcore::TrajectoryPoint(810, 69.608906867327),
+    Boardcore::TrajectoryPoint(820, 68.1063288926016),
+    Boardcore::TrajectoryPoint(830, 66.5731878070125),
+    Boardcore::TrajectoryPoint(840, 65.0072982469998),
+    Boardcore::TrajectoryPoint(850, 63.4062675866591),
+    Boardcore::TrajectoryPoint(860, 61.7672946721279),
+    Boardcore::TrajectoryPoint(870, 60.0867092471352),
+    Boardcore::TrajectoryPoint(880, 58.3618498819148),
+    Boardcore::TrajectoryPoint(890, 56.5883527365107),
+    Boardcore::TrajectoryPoint(900, 54.7612186747232),
+    Boardcore::TrajectoryPoint(910, 52.8749422119245),
+    Boardcore::TrajectoryPoint(920, 50.9233203445957),
+    Boardcore::TrajectoryPoint(930, 48.8982217335666),
+    Boardcore::TrajectoryPoint(940, 46.7901317777266),
+    Boardcore::TrajectoryPoint(950, 44.5878471465108),
+    Boardcore::TrajectoryPoint(960, 42.2751593304123),
+    Boardcore::TrajectoryPoint(970, 39.8344371101021),
+    Boardcore::TrajectoryPoint(980, 37.239897227397),
+    Boardcore::TrajectoryPoint(990, 34.4573271769189),
+    Boardcore::TrajectoryPoint(1000, 31.4358588250603),
+    Boardcore::TrajectoryPoint(1010, 28.1000266661281),
+    Boardcore::TrajectoryPoint(1020, 24.3185901290729),
+    Boardcore::TrajectoryPoint(1030, 19.8419180944302),
+    Boardcore::TrajectoryPoint(1040, 14.0181773597103),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t5_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.106833201762),
+    Boardcore::TrajectoryPoint(10, 151.297039405745),
+    Boardcore::TrajectoryPoint(20, 150.48493063729),
+    Boardcore::TrajectoryPoint(30, 149.670222724698),
+    Boardcore::TrajectoryPoint(40, 148.852624916669),
+    Boardcore::TrajectoryPoint(50, 148.032470723006),
+    Boardcore::TrajectoryPoint(60, 147.209527788255),
+    Boardcore::TrajectoryPoint(70, 146.38359222102),
+    Boardcore::TrajectoryPoint(80, 145.554973558463),
+    Boardcore::TrajectoryPoint(90, 144.723259864863),
+    Boardcore::TrajectoryPoint(100, 143.888550930633),
+    Boardcore::TrajectoryPoint(110, 143.050996121095),
+    Boardcore::TrajectoryPoint(120, 142.209969621106),
+    Boardcore::TrajectoryPoint(130, 141.366027260764),
+    Boardcore::TrajectoryPoint(140, 140.51872014465),
+    Boardcore::TrajectoryPoint(150, 139.668120078376),
+    Boardcore::TrajectoryPoint(160, 138.814268958225),
+    Boardcore::TrajectoryPoint(170, 137.95676010678),
+    Boardcore::TrajectoryPoint(180, 137.096057165278),
+    Boardcore::TrajectoryPoint(190, 136.23137761717),
+    Boardcore::TrajectoryPoint(200, 135.363413456239),
+    Boardcore::TrajectoryPoint(210, 134.491375541323),
+    Boardcore::TrajectoryPoint(220, 133.61584910905),
+    Boardcore::TrajectoryPoint(230, 132.736127930665),
+    Boardcore::TrajectoryPoint(240, 131.852748869473),
+    Boardcore::TrajectoryPoint(250, 130.964978298512),
+    Boardcore::TrajectoryPoint(260, 130.073443228631),
+    Boardcore::TrajectoryPoint(270, 129.177237835939),
+    Boardcore::TrajectoryPoint(280, 128.277068325644),
+    Boardcore::TrajectoryPoint(290, 127.372183490842),
+    Boardcore::TrajectoryPoint(300, 126.462946780625),
+    Boardcore::TrajectoryPoint(310, 125.549055898689),
+    Boardcore::TrajectoryPoint(320, 124.630311078864),
+    Boardcore::TrajectoryPoint(330, 123.707008104449),
+    Boardcore::TrajectoryPoint(340, 122.778339357199),
+    Boardcore::TrajectoryPoint(350, 121.844800990708),
+    Boardcore::TrajectoryPoint(360, 120.906167261799),
+    Boardcore::TrajectoryPoint(370, 119.961887469133),
+    Boardcore::TrajectoryPoint(380, 119.012358250267),
+    Boardcore::TrajectoryPoint(390, 118.057329741742),
+    Boardcore::TrajectoryPoint(400, 117.096219871632),
+    Boardcore::TrajectoryPoint(410, 116.12938870134),
+    Boardcore::TrajectoryPoint(420, 115.156663047603),
+    Boardcore::TrajectoryPoint(430, 114.177536676095),
+    Boardcore::TrajectoryPoint(440, 113.191965719725),
+    Boardcore::TrajectoryPoint(450, 112.199961506518),
+    Boardcore::TrajectoryPoint(460, 111.201324618268),
+    Boardcore::TrajectoryPoint(470, 110.195792194847),
+    Boardcore::TrajectoryPoint(480, 109.182873892645),
+    Boardcore::TrajectoryPoint(490, 108.162705825608),
+    Boardcore::TrajectoryPoint(500, 107.135057682957),
+    Boardcore::TrajectoryPoint(510, 106.099690023847),
+    Boardcore::TrajectoryPoint(520, 105.056353840069),
+    Boardcore::TrajectoryPoint(530, 104.004790092659),
+    Boardcore::TrajectoryPoint(540, 102.944729220586),
+    Boardcore::TrajectoryPoint(550, 101.875890619612),
+    Boardcore::TrajectoryPoint(560, 100.797982089216),
+    Boardcore::TrajectoryPoint(570, 99.7106992453148),
+    Boardcore::TrajectoryPoint(580, 98.6137248963344),
+    Boardcore::TrajectoryPoint(590, 97.5067283799706),
+    Boardcore::TrajectoryPoint(600, 96.3893648577493),
+    Boardcore::TrajectoryPoint(610, 95.2612745642613),
+    Boardcore::TrajectoryPoint(620, 94.1220820076657),
+    Boardcore::TrajectoryPoint(630, 92.9713951177634),
+    Boardcore::TrajectoryPoint(640, 91.8088043376144),
+    Boardcore::TrajectoryPoint(650, 90.6338816543104),
+    Boardcore::TrajectoryPoint(660, 89.4459948763227),
+    Boardcore::TrajectoryPoint(670, 88.2443601579958),
+    Boardcore::TrajectoryPoint(680, 87.0288866688643),
+    Boardcore::TrajectoryPoint(690, 85.7990547517776),
+    Boardcore::TrajectoryPoint(700, 84.5535680197478),
+    Boardcore::TrajectoryPoint(710, 83.2922484899762),
+    Boardcore::TrajectoryPoint(720, 82.0145260493254),
+    Boardcore::TrajectoryPoint(730, 80.7187949758998),
+    Boardcore::TrajectoryPoint(740, 79.4053282695061),
+    Boardcore::TrajectoryPoint(750, 78.071892898809),
+    Boardcore::TrajectoryPoint(760, 76.7186874528815),
+    Boardcore::TrajectoryPoint(770, 75.3438260854402),
+    Boardcore::TrajectoryPoint(780, 73.9463174159319),
+    Boardcore::TrajectoryPoint(790, 72.5251807926841),
+    Boardcore::TrajectoryPoint(800, 71.0787584173257),
+    Boardcore::TrajectoryPoint(810, 69.6049750292964),
+    Boardcore::TrajectoryPoint(820, 68.1026232997819),
+    Boardcore::TrajectoryPoint(830, 66.5697033126382),
+    Boardcore::TrajectoryPoint(840, 65.0040296599796),
+    Boardcore::TrajectoryPoint(850, 63.4032096739809),
+    Boardcore::TrajectoryPoint(860, 61.7644508959707),
+    Boardcore::TrajectoryPoint(870, 60.0840651290504),
+    Boardcore::TrajectoryPoint(880, 58.3594000541716),
+    Boardcore::TrajectoryPoint(890, 56.5860994386859),
+    Boardcore::TrajectoryPoint(900, 54.7591483275259),
+    Boardcore::TrajectoryPoint(910, 52.873056184392),
+    Boardcore::TrajectoryPoint(920, 50.9216122486964),
+    Boardcore::TrajectoryPoint(930, 48.8966850709917),
+    Boardcore::TrajectoryPoint(940, 46.7887599474286),
+    Boardcore::TrajectoryPoint(950, 44.5866334524767),
+    Boardcore::TrajectoryPoint(960, 42.2741019817903),
+    Boardcore::TrajectoryPoint(970, 39.8335283498377),
+    Boardcore::TrajectoryPoint(980, 37.2391290978975),
+    Boardcore::TrajectoryPoint(990, 34.4566953857729),
+    Boardcore::TrajectoryPoint(1000, 31.4353541101093),
+    Boardcore::TrajectoryPoint(1010, 28.0996394844248),
+    Boardcore::TrajectoryPoint(1020, 24.3183155799728),
+    Boardcore::TrajectoryPoint(1030, 19.8417459122337),
+    Boardcore::TrajectoryPoint(1040, 14.0180977761605),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t6_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.072783987011),
+    Boardcore::TrajectoryPoint(10, 151.263505617113),
+    Boardcore::TrajectoryPoint(20, 150.451887042568),
+    Boardcore::TrajectoryPoint(30, 149.637697884841),
+    Boardcore::TrajectoryPoint(40, 148.820583329703),
+    Boardcore::TrajectoryPoint(50, 148.000909302239),
+    Boardcore::TrajectoryPoint(60, 147.178474442449),
+    Boardcore::TrajectoryPoint(70, 146.353012133874),
+    Boardcore::TrajectoryPoint(80, 145.524863669696),
+    Boardcore::TrajectoryPoint(90, 144.693647313153),
+    Boardcore::TrajectoryPoint(100, 143.859401704909),
+    Boardcore::TrajectoryPoint(110, 143.022336542448),
+    Boardcore::TrajectoryPoint(120, 142.1817672384),
+    Boardcore::TrajectoryPoint(130, 141.338278334178),
+    Boardcore::TrajectoryPoint(140, 140.491450751199),
+    Boardcore::TrajectoryPoint(150, 139.641297325649),
+    Boardcore::TrajectoryPoint(160, 138.787918672288),
+    Boardcore::TrajectoryPoint(170, 137.93084966761),
+    Boardcore::TrajectoryPoint(180, 137.070606615013),
+    Boardcore::TrajectoryPoint(190, 136.206365609372),
+    Boardcore::TrajectoryPoint(200, 135.338831566631),
+    Boardcore::TrajectoryPoint(210, 134.467248065573),
+    Boardcore::TrajectoryPoint(220, 133.592145019571),
+    Boardcore::TrajectoryPoint(230, 132.712871074855),
+    Boardcore::TrajectoryPoint(240, 131.829908691435),
+    Boardcore::TrajectoryPoint(250, 130.942578141729),
+    Boardcore::TrajectoryPoint(260, 130.05145306502),
+    Boardcore::TrajectoryPoint(270, 129.155680452117),
+    Boardcore::TrajectoryPoint(280, 128.255940216178),
+    Boardcore::TrajectoryPoint(290, 127.351454952101),
+    Boardcore::TrajectoryPoint(300, 126.44264021665),
+    Boardcore::TrajectoryPoint(310, 125.529142278402),
+    Boardcore::TrajectoryPoint(320, 124.610812112527),
+    Boardcore::TrajectoryPoint(330, 123.687920195787),
+    Boardcore::TrajectoryPoint(340, 122.759634047109),
+    Boardcore::TrajectoryPoint(350, 121.826499372384),
+    Boardcore::TrajectoryPoint(360, 120.888241675445),
+    Boardcore::TrajectoryPoint(370, 119.944358193853),
+    Boardcore::TrajectoryPoint(380, 118.995221614201),
+    Boardcore::TrajectoryPoint(390, 118.040558875306),
+    Boardcore::TrajectoryPoint(400, 117.079834229482),
+    Boardcore::TrajectoryPoint(410, 116.113384567652),
+    Boardcore::TrajectoryPoint(420, 115.141036684993),
+    Boardcore::TrajectoryPoint(430, 114.162262164702),
+    Boardcore::TrajectoryPoint(440, 113.177061518822),
+    Boardcore::TrajectoryPoint(450, 112.185423841649),
+    Boardcore::TrajectoryPoint(460, 111.187149696296),
+    Boardcore::TrajectoryPoint(470, 110.18195528947),
+    Boardcore::TrajectoryPoint(480, 109.16939223526),
+    Boardcore::TrajectoryPoint(490, 108.149575592063),
+    Boardcore::TrajectoryPoint(500, 107.122275032974),
+    Boardcore::TrajectoryPoint(510, 106.087251101748),
+    Boardcore::TrajectoryPoint(520, 105.044254775484),
+    Boardcore::TrajectoryPoint(530, 103.993027001206),
+    Boardcore::TrajectoryPoint(540, 102.933298204535),
+    Boardcore::TrajectoryPoint(550, 101.864787768529),
+    Boardcore::TrajectoryPoint(560, 100.787203480583),
+    Boardcore::TrajectoryPoint(570, 99.7002409451403),
+    Boardcore::TrajectoryPoint(580, 98.6035829597442),
+    Boardcore::TrajectoryPoint(590, 97.4968988517833),
+    Boardcore::TrajectoryPoint(600, 96.3798437730387),
+    Boardcore::TrajectoryPoint(610, 95.2520579489053),
+    Boardcore::TrajectoryPoint(620, 94.1131658788825),
+    Boardcore::TrajectoryPoint(630, 92.9627754846369),
+    Boardcore::TrajectoryPoint(640, 91.8004772016072),
+    Boardcore::TrajectoryPoint(650, 90.6258430097677),
+    Boardcore::TrajectoryPoint(660, 89.4382558965152),
+    Boardcore::TrajectoryPoint(670, 88.2369011894616),
+    Boardcore::TrajectoryPoint(680, 87.0217036753287),
+    Boardcore::TrajectoryPoint(690, 85.7921436920914),
+    Boardcore::TrajectoryPoint(700, 84.5469386805745),
+    Boardcore::TrajectoryPoint(710, 83.2858824830871),
+    Boardcore::TrajectoryPoint(720, 82.0084326088468),
+    Boardcore::TrajectoryPoint(730, 80.7129561999655),
+    Boardcore::TrajectoryPoint(740, 79.3997527560275),
+    Boardcore::TrajectoryPoint(750, 78.0665633240034),
+    Boardcore::TrajectoryPoint(760, 76.7136116997284),
+    Boardcore::TrajectoryPoint(770, 75.3389874946048),
+    Boardcore::TrajectoryPoint(780, 73.941723081457),
+    Boardcore::TrajectoryPoint(790, 72.5208257784082),
+    Boardcore::TrajectoryPoint(800, 71.0746270039484),
+    Boardcore::TrajectoryPoint(810, 69.601073183387),
+    Boardcore::TrajectoryPoint(820, 68.098945961506),
+    Boardcore::TrajectoryPoint(830, 66.5662453757153),
+    Boardcore::TrajectoryPoint(840, 65.0007859741282),
+    Boardcore::TrajectoryPoint(850, 63.4001750472988),
+    Boardcore::TrajectoryPoint(860, 61.7616287672105),
+    Boardcore::TrajectoryPoint(870, 60.0814411294299),
+    Boardcore::TrajectoryPoint(880, 58.3569688580214),
+    Boardcore::TrajectoryPoint(890, 56.5838632709901),
+    Boardcore::TrajectoryPoint(900, 54.757093712143),
+    Boardcore::TrajectoryPoint(910, 52.8711844821095),
+    Boardcore::TrajectoryPoint(920, 50.9199171209883),
+    Boardcore::TrajectoryPoint(930, 48.8951600698814),
+    Boardcore::TrajectoryPoint(940, 46.7873985229568),
+    Boardcore::TrajectoryPoint(950, 44.5854289604041),
+    Boardcore::TrajectoryPoint(960, 42.2730526462557),
+    Boardcore::TrajectoryPoint(970, 39.8326264734145),
+    Boardcore::TrajectoryPoint(980, 37.2383667841264),
+    Boardcore::TrajectoryPoint(990, 34.4560683758031),
+    Boardcore::TrajectoryPoint(1000, 31.4348532126553),
+    Boardcore::TrajectoryPoint(1010, 28.0992552295128),
+    Boardcore::TrajectoryPoint(1020, 24.3180431049642),
+    Boardcore::TrajectoryPoint(1030, 19.8415750298029),
+    Boardcore::TrajectoryPoint(1040, 14.0180187927368),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t7_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.039000927416),
+    Boardcore::TrajectoryPoint(10, 151.23023494639),
+    Boardcore::TrajectoryPoint(20, 150.419102596577),
+    Boardcore::TrajectoryPoint(30, 149.605428073866),
+    Boardcore::TrajectoryPoint(40, 148.78879286286),
+    Boardcore::TrajectoryPoint(50, 147.969595120189),
+    Boardcore::TrajectoryPoint(60, 147.147664303406),
+    Boardcore::TrajectoryPoint(70, 146.322671432139),
+    Boardcore::TrajectoryPoint(80, 145.49498937204),
+    Boardcore::TrajectoryPoint(90, 144.664266409254),
+    Boardcore::TrajectoryPoint(100, 143.830480392353),
+    Boardcore::TrajectoryPoint(110, 142.993871814025),
+    Boardcore::TrajectoryPoint(120, 142.153785206939),
+    Boardcore::TrajectoryPoint(130, 141.310746110176),
+    Boardcore::TrajectoryPoint(140, 140.464394263504),
+    Boardcore::TrajectoryPoint(150, 139.614683889021),
+    Boardcore::TrajectoryPoint(160, 138.761773963392),
+    Boardcore::TrajectoryPoint(170, 137.90514127457),
+    Boardcore::TrajectoryPoint(180, 137.045331590065),
+    Boardcore::TrajectoryPoint(190, 136.181548494497),
+    Boardcore::TrajectoryPoint(200, 135.314441123351),
+    Boardcore::TrajectoryPoint(210, 134.443308446477),
+    Boardcore::TrajectoryPoint(220, 133.56862539815),
+    Boardcore::TrajectoryPoint(230, 132.689795156474),
+    Boardcore::TrajectoryPoint(240, 131.807246119943),
+    Boardcore::TrajectoryPoint(250, 130.920352120252),
+    Boardcore::TrajectoryPoint(260, 130.029633763249),
+    Boardcore::TrajectoryPoint(270, 129.134290518578),
+    Boardcore::TrajectoryPoint(280, 128.234976173414),
+    Boardcore::TrajectoryPoint(290, 127.330887295676),
+    Boardcore::TrajectoryPoint(300, 126.422491211683),
+    Boardcore::TrajectoryPoint(310, 125.509383089424),
+    Boardcore::TrajectoryPoint(320, 124.591464314658),
+    Boardcore::TrajectoryPoint(330, 123.668980000697),
+    Boardcore::TrajectoryPoint(340, 122.741073631931),
+    Boardcore::TrajectoryPoint(350, 121.808339475929),
+    Boardcore::TrajectoryPoint(360, 120.870454827233),
+    Boardcore::TrajectoryPoint(370, 119.926964544654),
+    Boardcore::TrajectoryPoint(380, 118.978217521817),
+    Boardcore::TrajectoryPoint(390, 118.023917655791),
+    Boardcore::TrajectoryPoint(400, 117.0635752131),
+    Boardcore::TrajectoryPoint(410, 116.097504068662),
+    Boardcore::TrajectoryPoint(420, 115.125530996297),
+    Boardcore::TrajectoryPoint(430, 114.147105547905),
+    Boardcore::TrajectoryPoint(440, 113.162272313311),
+    Boardcore::TrajectoryPoint(450, 112.170998303582),
+    Boardcore::TrajectoryPoint(460, 111.173084063324),
+    Boardcore::TrajectoryPoint(470, 110.168225010042),
+    Boardcore::TrajectoryPoint(480, 109.156014427786),
+    Boardcore::TrajectoryPoint(490, 108.136546463427),
+    Boardcore::TrajectoryPoint(500, 107.109590774075),
+    Boardcore::TrajectoryPoint(510, 106.074907888216),
+    Boardcore::TrajectoryPoint(520, 105.032248768379),
+    Boardcore::TrajectoryPoint(530, 103.981354347692),
+    Boardcore::TrajectoryPoint(540, 102.921955038534),
+    Boardcore::TrajectoryPoint(550, 101.853770211353),
+    Boardcore::TrajectoryPoint(560, 100.776507641556),
+    Boardcore::TrajectoryPoint(570, 99.6898629221948),
+    Boardcore::TrajectoryPoint(580, 98.5935188400059),
+    Boardcore::TrajectoryPoint(590, 97.4871447121413),
+    Boardcore::TrajectoryPoint(600, 96.3703956807007),
+    Boardcore::TrajectoryPoint(610, 95.24291196194),
+    Boardcore::TrajectoryPoint(620, 94.1043180467503),
+    Boardcore::TrajectoryPoint(630, 92.9542218487082),
+    Boardcore::TrajectoryPoint(640, 91.7922137956711),
+    Boardcore::TrajectoryPoint(650, 90.6178658605292),
+    Boardcore::TrajectoryPoint(660, 89.4305761007826),
+    Boardcore::TrajectoryPoint(670, 88.2294992382759),
+    Boardcore::TrajectoryPoint(680, 87.0145755649535),
+    Boardcore::TrajectoryPoint(690, 85.7852854139251),
+    Boardcore::TrajectoryPoint(700, 84.5403599541099),
+    Boardcore::TrajectoryPoint(710, 83.2795650561241),
+    Boardcore::TrajectoryPoint(720, 82.0023856518145),
+    Boardcore::TrajectoryPoint(730, 80.7071619439913),
+    Boardcore::TrajectoryPoint(740, 79.3942197395534),
+    Boardcore::TrajectoryPoint(750, 78.0612743523641),
+    Boardcore::TrajectoryPoint(760, 76.7085746013617),
+    Boardcore::TrajectoryPoint(770, 75.3341857346371),
+    Boardcore::TrajectoryPoint(780, 73.9371637049641),
+    Boardcore::TrajectoryPoint(790, 72.51650388797),
+    Boardcore::TrajectoryPoint(800, 71.0705269981542),
+    Boardcore::TrajectoryPoint(810, 69.5972009877557),
+    Boardcore::TrajectoryPoint(820, 68.0952965558671),
+    Boardcore::TrajectoryPoint(830, 66.562813693798),
+    Boardcore::TrajectoryPoint(840, 64.9975669059823),
+    Boardcore::TrajectoryPoint(850, 63.3971634416501),
+    Boardcore::TrajectoryPoint(860, 61.7588280396179),
+    Boardcore::TrajectoryPoint(870, 60.0788370195371),
+    Boardcore::TrajectoryPoint(880, 58.354556081729),
+    Boardcore::TrajectoryPoint(890, 56.5816440388267),
+    Boardcore::TrajectoryPoint(900, 54.7550546499471),
+    Boardcore::TrajectoryPoint(910, 52.8693269424873),
+    Boardcore::TrajectoryPoint(920, 50.9182348143468),
+    Boardcore::TrajectoryPoint(930, 48.893646597994),
+    Boardcore::TrajectoryPoint(940, 46.7860473863621),
+    Boardcore::TrajectoryPoint(950, 44.5842335660382),
+    Boardcore::TrajectoryPoint(960, 42.2720112330624),
+    Boardcore::TrajectoryPoint(970, 39.8317314029105),
+    Boardcore::TrajectoryPoint(980, 37.237610220284),
+    Boardcore::TrajectoryPoint(990, 34.4554460929404),
+    Boardcore::TrajectoryPoint(1000, 31.4343560895499),
+    Boardcore::TrajectoryPoint(1010, 28.0988738683303),
+    Boardcore::TrajectoryPoint(1020, 24.317772680632),
+    Boardcore::TrajectoryPoint(1030, 19.8414054324756),
+    Boardcore::TrajectoryPoint(1040, 14.017940402677),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t8_closed[] = {
+    Boardcore::TrajectoryPoint(0, 152.005480922458),
+    Boardcore::TrajectoryPoint(10, 151.197224313278),
+    Boardcore::TrajectoryPoint(20, 150.38657426689),
+    Boardcore::TrajectoryPoint(30, 149.573410307954),
+    Boardcore::TrajectoryPoint(40, 148.757250579419),
+    Boardcore::TrajectoryPoint(50, 147.938525286881),
+    Boardcore::TrajectoryPoint(60, 147.117094528702),
+    Boardcore::TrajectoryPoint(70, 146.292567319369),
+    Boardcore::TrajectoryPoint(80, 145.465347914672),
+    Boardcore::TrajectoryPoint(90, 144.635114448831),
+    Boardcore::TrajectoryPoint(100, 143.801784333495),
+    Boardcore::TrajectoryPoint(110, 142.965628675919),
+    Boardcore::TrajectoryPoint(120, 142.126020957175),
+    Boardcore::TrajectoryPoint(130, 141.283428062975),
+    Boardcore::TrajectoryPoint(140, 140.437548200509),
+    Boardcore::TrajectoryPoint(150, 139.588277330443),
+    Boardcore::TrajectoryPoint(160, 138.735832437525),
+    Boardcore::TrajectoryPoint(170, 137.879632575911),
+    Boardcore::TrajectoryPoint(180, 137.020252796546),
+    Boardcore::TrajectoryPoint(190, 136.156924005649),
+    Boardcore::TrajectoryPoint(200, 135.290239900686),
+    Boardcore::TrajectoryPoint(210, 134.419554500541),
+    Boardcore::TrajectoryPoint(220, 133.545288101743),
+    Boardcore::TrajectoryPoint(230, 132.666898073987),
+    Boardcore::TrajectoryPoint(240, 131.784759093177),
+    Boardcore::TrajectoryPoint(250, 130.898298213049),
+    Boardcore::TrajectoryPoint(260, 130.007983341272),
+    Boardcore::TrajectoryPoint(270, 129.113066093345),
+    Boardcore::TrajectoryPoint(280, 128.21417429511),
+    Boardcore::TrajectoryPoint(290, 127.310478657188),
+    Boardcore::TrajectoryPoint(300, 126.402497940352),
+    Boardcore::TrajectoryPoint(310, 125.489776543526),
+    Boardcore::TrajectoryPoint(320, 124.572265935296),
+    Boardcore::TrajectoryPoint(330, 123.650161857034),
+    Boardcore::TrajectoryPoint(340, 122.722656435667),
+    Boardcore::TrajectoryPoint(350, 121.79031966253),
+    Boardcore::TrajectoryPoint(360, 120.852805113679),
+    Boardcore::TrajectoryPoint(370, 119.909704954489),
+    Boardcore::TrajectoryPoint(380, 118.961344442155),
+    Boardcore::TrajectoryPoint(390, 118.007404586476),
+    Boardcore::TrajectoryPoint(400, 117.047441361105),
+    Boardcore::TrajectoryPoint(410, 116.081745777964),
+    Boardcore::TrajectoryPoint(420, 115.110144589725),
+    Boardcore::TrajectoryPoint(430, 114.132065466685),
+    Boardcore::TrajectoryPoint(440, 113.147596778033),
+    Boardcore::TrajectoryPoint(450, 112.156683600653),
+    Boardcore::TrajectoryPoint(460, 111.159126460812),
+    Boardcore::TrajectoryPoint(470, 110.154600129346),
+    Boardcore::TrajectoryPoint(480, 109.142739275375),
+    Boardcore::TrajectoryPoint(490, 108.12361727685),
+    Boardcore::TrajectoryPoint(500, 107.097003775031),
+    Boardcore::TrajectoryPoint(510, 106.062659283267),
+    Boardcore::TrajectoryPoint(520, 105.020334749637),
+    Boardcore::TrajectoryPoint(530, 103.969771093486),
+    Boardcore::TrajectoryPoint(540, 102.910698714053),
+    Boardcore::TrajectoryPoint(550, 101.842836969278),
+    Boardcore::TrajectoryPoint(560, 100.765893622667),
+    Boardcore::TrajectoryPoint(570, 99.6795642559619),
+    Boardcore::TrajectoryPoint(580, 98.5835316451706),
+    Boardcore::TrajectoryPoint(590, 97.4774650972766),
+    Boardcore::TrajectoryPoint(600, 96.361019744761),
+    Boardcore::TrajectoryPoint(610, 95.2338357947974),
+    Boardcore::TrajectoryPoint(620, 94.0955377297188),
+    Boardcore::TrajectoryPoint(630, 92.9457334550571),
+    Boardcore::TrajectoryPoint(640, 91.7840133911267),
+    Boardcore::TrajectoryPoint(650, 90.6099495037675),
+    Boardcore::TrajectoryPoint(660, 89.4229548129136),
+    Boardcore::TrajectoryPoint(670, 88.2221536532672),
+    Boardcore::TrajectoryPoint(680, 87.0075017112168),
+    Boardcore::TrajectoryPoint(690, 85.7784793150158),
+    Boardcore::TrajectoryPoint(700, 84.5338312630252),
+    Boardcore::TrajectoryPoint(710, 83.2732956551972),
+    Boardcore::TrajectoryPoint(720, 81.9963846484215),
+    Boardcore::TrajectoryPoint(730, 80.7014117007834),
+    Boardcore::TrajectoryPoint(740, 79.3887287361081),
+    Boardcore::TrajectoryPoint(750, 78.0560255217003),
+    Boardcore::TrajectoryPoint(760, 76.7035757179309),
+    Boardcore::TrajectoryPoint(770, 75.3294203866412),
+    Boardcore::TrajectoryPoint(780, 73.9326388890096),
+    Boardcore::TrajectoryPoint(790, 72.5122147449245),
+    Boardcore::TrajectoryPoint(800, 71.0664580431786),
+    Boardcore::TrajectoryPoint(810, 69.5933581057342),
+    Boardcore::TrajectoryPoint(820, 68.0916747658291),
+    Boardcore::TrajectoryPoint(830, 66.5594079690151),
+    Boardcore::TrajectoryPoint(840, 64.9943721763642),
+    Boardcore::TrajectoryPoint(850, 63.3941745960764),
+    Boardcore::TrajectoryPoint(860, 61.7560484706829),
+    Boardcore::TrajectoryPoint(870, 60.0762525740897),
+    Boardcore::TrajectoryPoint(880, 58.3521615167551),
+    Boardcore::TrajectoryPoint(890, 56.5794415505358),
+    Boardcore::TrajectoryPoint(900, 54.7530309650049),
+    Boardcore::TrajectoryPoint(910, 52.8674834053872),
+    Boardcore::TrajectoryPoint(920, 50.9165651838645),
+    Boardcore::TrajectoryPoint(930, 48.89214452508),
+    Boardcore::TrajectoryPoint(940, 46.7847064214711),
+    Boardcore::TrajectoryPoint(950, 44.5830471666934),
+    Boardcore::TrajectoryPoint(960, 42.2709776528296),
+    Boardcore::TrajectoryPoint(970, 39.8308430615754),
+    Boardcore::TrajectoryPoint(980, 37.2368593415597),
+    Boardcore::TrajectoryPoint(990, 34.4548284839283),
+    Boardcore::TrajectoryPoint(1000, 31.4338626982924),
+    Boardcore::TrajectoryPoint(1010, 28.0984953683119),
+    Boardcore::TrajectoryPoint(1020, 24.3175042839124),
+    Boardcore::TrajectoryPoint(1030, 19.841237105809),
+    Boardcore::TrajectoryPoint(1040, 14.0178625993186),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t9_closed[] = {
+    Boardcore::TrajectoryPoint(0, 151.972220919451),
+    Boardcore::TrajectoryPoint(10, 151.1644706853),
+    Boardcore::TrajectoryPoint(20, 150.354299068137),
+    Boardcore::TrajectoryPoint(30, 149.541641649582),
+    Boardcore::TrajectoryPoint(40, 148.725953588209),
+    Boardcore::TrajectoryPoint(50, 147.907696957141),
+    Boardcore::TrajectoryPoint(60, 147.086762319979),
+    Boardcore::TrajectoryPoint(70, 146.26269704245),
+    Boardcore::TrajectoryPoint(80, 145.435936589378),
+    Boardcore::TrajectoryPoint(90, 144.606188769432),
+    Boardcore::TrajectoryPoint(100, 143.773310910031),
+    Boardcore::TrajectoryPoint(110, 142.937604553633),
+    Boardcore::TrajectoryPoint(120, 142.098471959315),
+    Boardcore::TrajectoryPoint(130, 141.256321705844),
+    Boardcore::TrajectoryPoint(140, 140.410910119514),
+    Boardcore::TrajectoryPoint(150, 139.562075249543),
+    Boardcore::TrajectoryPoint(160, 138.710091737666),
+    Boardcore::TrajectoryPoint(170, 137.854321256199),
+    Boardcore::TrajectoryPoint(180, 136.995367960286),
+    Boardcore::TrajectoryPoint(190, 136.132489910916),
+    Boardcore::TrajectoryPoint(200, 135.266225707255),
+    Boardcore::TrajectoryPoint(210, 134.395984077953),
+    Boardcore::TrajectoryPoint(220, 133.52213102034),
+    Boardcore::TrajectoryPoint(230, 132.644177758248),
+    Boardcore::TrajectoryPoint(240, 131.762445581076),
+    Boardcore::TrajectoryPoint(250, 130.876414430218),
+    Boardcore::TrajectoryPoint(260, 129.986499847555),
+    Boardcore::TrajectoryPoint(270, 129.092005264325),
+    Boardcore::TrajectoryPoint(280, 128.193532708298),
+    Boardcore::TrajectoryPoint(290, 127.29022720093),
+    Boardcore::TrajectoryPoint(300, 126.382658605351),
+    Boardcore::TrajectoryPoint(310, 125.47032087996),
+    Boardcore::TrajectoryPoint(320, 124.553215251368),
+    Boardcore::TrajectoryPoint(330, 123.631488409761),
+    Boardcore::TrajectoryPoint(340, 122.704380808049),
+    Boardcore::TrajectoryPoint(350, 121.772438318524),
+    Boardcore::TrajectoryPoint(360, 120.8352909559),
+    Boardcore::TrajectoryPoint(370, 119.892577880348),
+    Boardcore::TrajectoryPoint(380, 118.944600867729),
+    Boardcore::TrajectoryPoint(390, 117.991018193578),
+    Boardcore::TrajectoryPoint(400, 117.031431234504),
+    Boardcore::TrajectoryPoint(410, 116.066108291001),
+    Boardcore::TrajectoryPoint(420, 115.094876094798),
+    Boardcore::TrajectoryPoint(430, 114.117140582819),
+    Boardcore::TrajectoryPoint(440, 113.133033608106),
+    Boardcore::TrajectoryPoint(450, 112.142478460955),
+    Boardcore::TrajectoryPoint(460, 111.145275649464),
+    Boardcore::TrajectoryPoint(470, 110.141079438919),
+    Boardcore::TrajectoryPoint(480, 109.129565601433),
+    Boardcore::TrajectoryPoint(490, 108.110786887239),
+    Boardcore::TrajectoryPoint(500, 107.084512921882),
+    Boardcore::TrajectoryPoint(510, 106.050504203703),
+    Boardcore::TrajectoryPoint(520, 105.008511666449),
+    Boardcore::TrajectoryPoint(530, 103.958276215791),
+    Boardcore::TrajectoryPoint(540, 102.899528237938),
+    Boardcore::TrajectoryPoint(550, 101.831987078412),
+    Boardcore::TrajectoryPoint(560, 100.755360488908),
+    Boardcore::TrajectoryPoint(570, 99.6693440399418),
+    Boardcore::TrajectoryPoint(580, 98.5736204968654),
+    Boardcore::TrajectoryPoint(590, 97.4678591565632),
+    Boardcore::TrajectoryPoint(600, 96.3517151419596),
+    Boardcore::TrajectoryPoint(610, 95.224828651202),
+    Boardcore::TrajectoryPoint(620, 94.0868241581155),
+    Boardcore::TrajectoryPoint(630, 92.9373095602312),
+    Boardcore::TrajectoryPoint(640, 91.7758752703596),
+    Boardcore::TrajectoryPoint(650, 90.6020932473235),
+    Boardcore::TrajectoryPoint(660, 89.4153913669583),
+    Boardcore::TrajectoryPoint(670, 88.2148637931413),
+    Boardcore::TrajectoryPoint(680, 87.0004814970956),
+    Boardcore::TrajectoryPoint(690, 85.7717248022281),
+    Boardcore::TrajectoryPoint(700, 84.5273520387386),
+    Boardcore::TrajectoryPoint(710, 83.267073734804),
+    Boardcore::TrajectoryPoint(720, 81.9904290768815),
+    Boardcore::TrajectoryPoint(730, 80.6957049708232),
+    Boardcore::TrajectoryPoint(740, 79.3832792690369),
+    Boardcore::TrajectoryPoint(750, 78.0508163768098),
+    Boardcore::TrajectoryPoint(760, 76.6986146162335),
+    Boardcore::TrajectoryPoint(770, 75.3246910380499),
+    Boardcore::TrajectoryPoint(780, 73.9281482421519),
+    Boardcore::TrajectoryPoint(790, 72.5079579785098),
+    Boardcore::TrajectoryPoint(800, 71.0624197876405),
+    Boardcore::TrajectoryPoint(810, 69.5895442057322),
+    Boardcore::TrajectoryPoint(820, 68.088080279136),
+    Boardcore::TrajectoryPoint(830, 66.5560279079846),
+    Boardcore::TrajectoryPoint(840, 64.9912015103021),
+    Boardcore::TrajectoryPoint(850, 63.3912082535489),
+    Boardcore::TrajectoryPoint(860, 61.7532898215466),
+    Boardcore::TrajectoryPoint(870, 60.0736875711954),
+    Boardcore::TrajectoryPoint(880, 58.3497849576973),
+    Boardcore::TrajectoryPoint(890, 56.577255617339),
+    Boardcore::TrajectoryPoint(900, 54.751022484027),
+    Boardcore::TrajectoryPoint(910, 52.8656537130767),
+    Boardcore::TrajectoryPoint(920, 50.9149080868102),
+    Boardcore::TrajectoryPoint(930, 48.8906537228448),
+    Boardcore::TrajectoryPoint(940, 46.7833755138536),
+    Boardcore::TrajectoryPoint(950, 44.5818696612242),
+    Boardcore::TrajectoryPoint(960, 42.2699518175164),
+    Boardcore::TrajectoryPoint(970, 39.8299613738092),
+    Boardcore::TrajectoryPoint(980, 37.2361140841138),
+    Boardcore::TrajectoryPoint(990, 34.4542154963078),
+    Boardcore::TrajectoryPoint(1000, 31.4333729970188),
+    Boardcore::TrajectoryPoint(1010, 28.0981196973795),
+    Boardcore::TrajectoryPoint(1020, 24.3172378920867),
+    Boardcore::TrajectoryPoint(1030, 19.8410700355767),
+    Boardcore::TrajectoryPoint(1040, 14.0177853760999),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t10_closed[] = {
+    Boardcore::TrajectoryPoint(0, 151.939217912631),
+    Boardcore::TrajectoryPoint(10, 151.131971076877),
+    Boardcore::TrajectoryPoint(20, 150.322274061098),
+    Boardcore::TrajectoryPoint(30, 149.510091542429),
+    Boardcore::TrajectoryPoint(40, 148.694899042729),
+    Boardcore::TrajectoryPoint(50, 147.877107329736),
+    Boardcore::TrajectoryPoint(60, 147.056664922089),
+    Boardcore::TrajectoryPoint(70, 146.23305789076),
+    Boardcore::TrajectoryPoint(80, 145.406752729722),
+    Boardcore::TrajectoryPoint(90, 144.577486749675),
+    Boardcore::TrajectoryPoint(100, 143.745057544031),
+    Boardcore::TrajectoryPoint(110, 142.909796912352),
+    Boardcore::TrajectoryPoint(120, 142.071135722547),
+    Boardcore::TrajectoryPoint(130, 141.229424590358),
+    Boardcore::TrajectoryPoint(140, 140.384477615441),
+    Boardcore::TrajectoryPoint(150, 139.536075282899),
+    Boardcore::TrajectoryPoint(160, 138.68454954307),
+    Boardcore::TrajectoryPoint(170, 137.829205035625),
+    Boardcore::TrajectoryPoint(180, 136.970674842081),
+    Boardcore::TrajectoryPoint(190, 136.108244012702),
+    Boardcore::TrajectoryPoint(200, 135.242396385354),
+    Boardcore::TrajectoryPoint(210, 134.372595061928),
+    Boardcore::TrajectoryPoint(220, 133.499152076336),
+    Boardcore::TrajectoryPoint(230, 132.621632171883),
+    Boardcore::TrajectoryPoint(240, 131.740303584736),
+    Boardcore::TrajectoryPoint(250, 130.854698812387),
+    Boardcore::TrajectoryPoint(260, 129.965181360491),
+    Boardcore::TrajectoryPoint(270, 129.071106148747),
+    Boardcore::TrajectoryPoint(280, 128.173049568719),
+    Boardcore::TrajectoryPoint(290, 127.270131119324),
+    Boardcore::TrajectoryPoint(300, 126.362971436904),
+    Boardcore::TrajectoryPoint(310, 125.451014364936),
+    Boardcore::TrajectoryPoint(320, 124.534310566176),
+    Boardcore::TrajectoryPoint(330, 123.612957997475),
+    Boardcore::TrajectoryPoint(340, 122.686245124051),
+    Boardcore::TrajectoryPoint(350, 121.754693854927),
+    Boardcore::TrajectoryPoint(360, 120.817910799138),
+    Boardcore::TrajectoryPoint(370, 119.875581802795),
+    Boardcore::TrajectoryPoint(380, 118.92798531408),
+    Boardcore::TrajectoryPoint(390, 117.974757025813),
+    Boardcore::TrajectoryPoint(400, 117.015543416263),
+    Boardcore::TrajectoryPoint(410, 116.050590224644),
+    Boardcore::TrajectoryPoint(420, 115.079724161938),
+    Boardcore::TrajectoryPoint(430, 114.102329578488),
+    Boardcore::TrajectoryPoint(440, 113.11858151853),
+    Boardcore::TrajectoryPoint(450, 112.128381631957),
+    Boardcore::TrajectoryPoint(460, 111.131530408858),
+    Boardcore::TrajectoryPoint(470, 110.127661748693),
+    Boardcore::TrajectoryPoint(480, 109.116492247272),
+    Boardcore::TrajectoryPoint(490, 108.098054166921),
+    Boardcore::TrajectoryPoint(500, 107.072117117609),
+    Boardcore::TrajectoryPoint(510, 106.038441582792),
+    Boardcore::TrajectoryPoint(520, 104.996778482003),
+    Boardcore::TrajectoryPoint(530, 103.946868707353),
+    Boardcore::TrajectoryPoint(540, 102.888442632115),
+    Boardcore::TrajectoryPoint(550, 101.821219589495),
+    Boardcore::TrajectoryPoint(560, 100.744907319461),
+    Boardcore::TrajectoryPoint(570, 99.6591936540695),
+    Boardcore::TrajectoryPoint(580, 98.5637845300357),
+    Boardcore::TrajectoryPoint(590, 97.4583260522675),
+    Boardcore::TrajectoryPoint(600, 96.3424810615093),
+    Boardcore::TrajectoryPoint(610, 95.2158897469375),
+    Boardcore::TrajectoryPoint(620, 94.078176573919),
+    Boardcore::TrajectoryPoint(630, 92.928949432028),
+    Boardcore::TrajectoryPoint(640, 91.7677987266097),
+    Boardcore::TrajectoryPoint(650, 90.5942964095027),
+    Boardcore::TrajectoryPoint(660, 89.4078851070323),
+    Boardcore::TrajectoryPoint(670, 88.2076290262928),
+    Boardcore::TrajectoryPoint(680, 86.9935143148854),
+    Boardcore::TrajectoryPoint(690, 85.7650212913802),
+    Boardcore::TrajectoryPoint(700, 84.5209217212486),
+    Boardcore::TrajectoryPoint(710, 83.2608987576706),
+    Boardcore::TrajectoryPoint(720, 81.9845184232764),
+    Boardcore::TrajectoryPoint(730, 80.6900412621207),
+    Boardcore::TrajectoryPoint(740, 79.3778708688667),
+    Boardcore::TrajectoryPoint(750, 78.0456464693453),
+    Boardcore::TrajectoryPoint(760, 76.6936908695887),
+    Boardcore::TrajectoryPoint(770, 75.319997282504),
+    Boardcore::TrajectoryPoint(780, 73.9236913788376),
+    Boardcore::TrajectoryPoint(790, 72.5037332235386),
+    Boardcore::TrajectoryPoint(800, 71.0584118854396),
+    Boardcore::TrajectoryPoint(810, 69.5857589611408),
+    Boardcore::TrajectoryPoint(820, 68.0845127882209),
+    Boardcore::TrajectoryPoint(830, 66.5526732217284),
+    Boardcore::TrajectoryPoint(840, 64.9880546369497),
+    Boardcore::TrajectoryPoint(850, 63.3882641608939),
+    Boardcore::TrajectoryPoint(860, 61.7505518569308),
+    Boardcore::TrajectoryPoint(870, 60.0711417922867),
+    Boardcore::TrajectoryPoint(880, 58.3474262022295),
+    Boardcore::TrajectoryPoint(890, 56.5750860532844),
+    Boardcore::TrajectoryPoint(900, 54.7490290363177),
+    Boardcore::TrajectoryPoint(910, 52.8638377101825),
+    Boardcore::TrajectoryPoint(920, 50.9132633825867),
+    Boardcore::TrajectoryPoint(930, 48.8891740649115),
+    Boardcore::TrajectoryPoint(940, 46.782054550789),
+    Boardcore::TrajectoryPoint(950, 44.5807009499952),
+    Boardcore::TrajectoryPoint(960, 42.2689336403958),
+    Boardcore::TrajectoryPoint(970, 39.8290862651394),
+    Boardcore::TrajectoryPoint(980, 37.2353743850582),
+    Boardcore::TrajectoryPoint(990, 34.4536070784014),
+    Boardcore::TrajectoryPoint(1000, 31.432886944488),
+    Boardcore::TrajectoryPoint(1010, 28.0977468239322),
+    Boardcore::TrajectoryPoint(1020, 24.316973482774),
+    Boardcore::TrajectoryPoint(1030, 19.8409042077629),
+    Boardcore::TrajectoryPoint(1040, 14.0177087265563),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t0_open[] = {
+    Boardcore::TrajectoryPoint(0, 169.360315331402),
+    Boardcore::TrajectoryPoint(10, 168.246480367201),
+    Boardcore::TrajectoryPoint(20, 167.134073503613),
+    Boardcore::TrajectoryPoint(30, 166.022426559672),
+    Boardcore::TrajectoryPoint(40, 164.911499807547),
+    Boardcore::TrajectoryPoint(50, 163.801180771595),
+    Boardcore::TrajectoryPoint(60, 162.691340150731),
+    Boardcore::TrajectoryPoint(70, 161.581892032395),
+    Boardcore::TrajectoryPoint(80, 160.472755991675),
+    Boardcore::TrajectoryPoint(90, 159.363858989209),
+    Boardcore::TrajectoryPoint(100, 158.255076329991),
+    Boardcore::TrajectoryPoint(110, 157.146323850155),
+    Boardcore::TrajectoryPoint(120, 156.037509424379),
+    Boardcore::TrajectoryPoint(130, 154.928523292648),
+    Boardcore::TrajectoryPoint(140, 153.819291188769),
+    Boardcore::TrajectoryPoint(150, 152.709730100639),
+    Boardcore::TrajectoryPoint(160, 151.599668963086),
+    Boardcore::TrajectoryPoint(170, 150.489079565894),
+    Boardcore::TrajectoryPoint(180, 149.377885385811),
+    Boardcore::TrajectoryPoint(190, 148.265870235037),
+    Boardcore::TrajectoryPoint(200, 147.153034470015),
+    Boardcore::TrajectoryPoint(210, 146.039310268352),
+    Boardcore::TrajectoryPoint(220, 144.924403754144),
+    Boardcore::TrajectoryPoint(230, 143.808418560157),
+    Boardcore::TrajectoryPoint(240, 142.691154752502),
+    Boardcore::TrajectoryPoint(250, 141.572461306612),
+    Boardcore::TrajectoryPoint(260, 140.452406425859),
+    Boardcore::TrajectoryPoint(270, 139.330559078558),
+    Boardcore::TrajectoryPoint(280, 138.207142240544),
+    Boardcore::TrajectoryPoint(290, 137.081788005055),
+    Boardcore::TrajectoryPoint(300, 135.954543762304),
+    Boardcore::TrajectoryPoint(310, 134.825185415819),
+    Boardcore::TrajectoryPoint(320, 133.693635972874),
+    Boardcore::TrajectoryPoint(330, 132.559748527893),
+    Boardcore::TrajectoryPoint(340, 131.423402685643),
+    Boardcore::TrajectoryPoint(350, 130.284432664314),
+    Boardcore::TrajectoryPoint(360, 129.142784634321),
+    Boardcore::TrajectoryPoint(370, 127.998149031994),
+    Boardcore::TrajectoryPoint(380, 126.850677124078),
+    Boardcore::TrajectoryPoint(390, 125.699762077224),
+    Boardcore::TrajectoryPoint(400, 124.545732350525),
+    Boardcore::TrajectoryPoint(410, 123.388086429243),
+    Boardcore::TrajectoryPoint(420, 122.22678244767),
+    Boardcore::TrajectoryPoint(430, 121.061838910406),
+    Boardcore::TrajectoryPoint(440, 119.892601854044),
+    Boardcore::TrajectoryPoint(450, 118.719294972083),
+    Boardcore::TrajectoryPoint(460, 117.541762424381),
+    Boardcore::TrajectoryPoint(470, 116.359420720709),
+    Boardcore::TrajectoryPoint(480, 115.172332529927),
+    Boardcore::TrajectoryPoint(490, 113.980366023733),
+    Boardcore::TrajectoryPoint(500, 112.78328457612),
+    Boardcore::TrajectoryPoint(510, 111.58046804476),
+    Boardcore::TrajectoryPoint(520, 110.372034891077),
+    Boardcore::TrajectoryPoint(530, 109.157731235397),
+    Boardcore::TrajectoryPoint(540, 107.937279056964),
+    Boardcore::TrajectoryPoint(550, 106.710388018682),
+    Boardcore::TrajectoryPoint(560, 105.476754945406),
+    Boardcore::TrajectoryPoint(570, 104.23588820423),
+    Boardcore::TrajectoryPoint(580, 102.987660747081),
+    Boardcore::TrajectoryPoint(590, 101.731742270957),
+    Boardcore::TrajectoryPoint(600, 100.467776711107),
+    Boardcore::TrajectoryPoint(610, 99.1953920596245),
+    Boardcore::TrajectoryPoint(620, 97.9141995654263),
+    Boardcore::TrajectoryPoint(630, 96.6237928747764),
+    Boardcore::TrajectoryPoint(640, 95.3237471084763),
+    Boardcore::TrajectoryPoint(650, 94.0134755817458),
+    Boardcore::TrajectoryPoint(660, 92.6924549638871),
+    Boardcore::TrajectoryPoint(670, 91.3603118747879),
+    Boardcore::TrajectoryPoint(680, 90.0165297318923),
+    Boardcore::TrajectoryPoint(690, 88.660567268661),
+    Boardcore::TrajectoryPoint(700, 87.2916139739664),
+    Boardcore::TrajectoryPoint(710, 85.908740671119),
+    Boardcore::TrajectoryPoint(720, 84.5117719408656),
+    Boardcore::TrajectoryPoint(730, 83.0996650103509),
+    Boardcore::TrajectoryPoint(740, 81.6712708674768),
+    Boardcore::TrajectoryPoint(750, 80.2264197090199),
+    Boardcore::TrajectoryPoint(760, 78.7630162208311),
+    Boardcore::TrajectoryPoint(770, 77.2811121656465),
+    Boardcore::TrajectoryPoint(780, 75.7786303881267),
+    Boardcore::TrajectoryPoint(790, 74.2547147333932),
+    Boardcore::TrajectoryPoint(800, 72.7081441558916),
+    Boardcore::TrajectoryPoint(810, 71.1368655836554),
+    Boardcore::TrajectoryPoint(820, 69.5391547447803),
+    Boardcore::TrajectoryPoint(830, 67.9133719216858),
+    Boardcore::TrajectoryPoint(840, 66.257311088182),
+    Boardcore::TrajectoryPoint(850, 64.5685515855946),
+    Boardcore::TrajectoryPoint(860, 62.8444325050821),
+    Boardcore::TrajectoryPoint(870, 61.0818038286117),
+    Boardcore::TrajectoryPoint(880, 59.2765618187081),
+    Boardcore::TrajectoryPoint(890, 57.4257151566554),
+    Boardcore::TrajectoryPoint(900, 55.523715252443),
+    Boardcore::TrajectoryPoint(910, 53.5660024963227),
+    Boardcore::TrajectoryPoint(920, 51.5446655475009),
+    Boardcore::TrajectoryPoint(930, 49.4529070771036),
+    Boardcore::TrajectoryPoint(940, 47.2813085630852),
+    Boardcore::TrajectoryPoint(950, 45.0173461901611),
+    Boardcore::TrajectoryPoint(960, 42.6472163924613),
+    Boardcore::TrajectoryPoint(970, 40.1512282774346),
+    Boardcore::TrajectoryPoint(980, 37.5049975932762),
+    Boardcore::TrajectoryPoint(990, 34.6731004141643),
+    Boardcore::TrajectoryPoint(1000, 31.6062887300925),
+    Boardcore::TrajectoryPoint(1010, 28.2286684462889),
+    Boardcore::TrajectoryPoint(1020, 24.4089685562411),
+    Boardcore::TrajectoryPoint(1030, 19.8977836278758),
+    Boardcore::TrajectoryPoint(1040, 14.0435849933506),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t1_open[] = {
+    Boardcore::TrajectoryPoint(0, 169.245401138596),
+    Boardcore::TrajectoryPoint(10, 168.13373829021),
+    Boardcore::TrajectoryPoint(20, 167.0234127897),
+    Boardcore::TrajectoryPoint(30, 165.913819215268),
+    Boardcore::TrajectoryPoint(40, 164.804894908062),
+    Boardcore::TrajectoryPoint(50, 163.696573631899),
+    Boardcore::TrajectoryPoint(60, 162.588704265396),
+    Boardcore::TrajectoryPoint(70, 161.481201390453),
+    Boardcore::TrajectoryPoint(80, 160.373980152089),
+    Boardcore::TrajectoryPoint(90, 159.266973787375),
+    Boardcore::TrajectoryPoint(100, 158.160059173274),
+    Boardcore::TrajectoryPoint(110, 157.053150545625),
+    Boardcore::TrajectoryPoint(120, 155.946160005792),
+    Boardcore::TrajectoryPoint(130, 154.838968124123),
+    Boardcore::TrajectoryPoint(140, 153.731507331512),
+    Boardcore::TrajectoryPoint(150, 152.623695012999),
+    Boardcore::TrajectoryPoint(160, 151.515370665724),
+    Boardcore::TrajectoryPoint(170, 150.406483238506),
+    Boardcore::TrajectoryPoint(180, 149.296969622668),
+    Boardcore::TrajectoryPoint(190, 148.186630337033),
+    Boardcore::TrajectoryPoint(200, 147.075430248138),
+    Boardcore::TrajectoryPoint(210, 145.963327820648),
+    Boardcore::TrajectoryPoint(220, 144.850030479483),
+    Boardcore::TrajectoryPoint(230, 143.735617545146),
+    Boardcore::TrajectoryPoint(240, 142.619932618686),
+    Boardcore::TrajectoryPoint(250, 141.502769476635),
+    Boardcore::TrajectoryPoint(260, 140.384226138456),
+    Boardcore::TrajectoryPoint(270, 139.26389865617),
+    Boardcore::TrajectoryPoint(280, 138.141953075543),
+    Boardcore::TrajectoryPoint(290, 137.018082242506),
+    Boardcore::TrajectoryPoint(300, 135.892269994398),
+    Boardcore::TrajectoryPoint(310, 134.764358670881),
+    Boardcore::TrajectoryPoint(320, 133.634202948614),
+    Boardcore::TrajectoryPoint(330, 132.501726313433),
+    Boardcore::TrajectoryPoint(340, 131.36673687076),
+    Boardcore::TrajectoryPoint(350, 130.229141677191),
+    Boardcore::TrajectoryPoint(360, 129.088813641437),
+    Boardcore::TrajectoryPoint(370, 127.945517165827),
+    Boardcore::TrajectoryPoint(380, 126.799329725144),
+    Boardcore::TrajectoryPoint(390, 125.649718422777),
+    Boardcore::TrajectoryPoint(400, 124.496976446168),
+    Boardcore::TrajectoryPoint(410, 123.34056126544),
+    Boardcore::TrajectoryPoint(420, 122.180509976902),
+    Boardcore::TrajectoryPoint(430, 121.016802951695),
+    Boardcore::TrajectoryPoint(440, 119.848744663861),
+    Boardcore::TrajectoryPoint(450, 118.676639666519),
+    Boardcore::TrajectoryPoint(460, 117.500292741596),
+    Boardcore::TrajectoryPoint(470, 116.319079500839),
+    Boardcore::TrajectoryPoint(480, 115.133142803974),
+    Boardcore::TrajectoryPoint(490, 113.942311527574),
+    Boardcore::TrajectoryPoint(500, 112.746322118768),
+    Boardcore::TrajectoryPoint(510, 111.544594927024),
+    Boardcore::TrajectoryPoint(520, 110.33724720946),
+    Boardcore::TrajectoryPoint(530, 109.124012748825),
+    Boardcore::TrajectoryPoint(540, 107.904613502543),
+    Boardcore::TrajectoryPoint(550, 106.678759122208),
+    Boardcore::TrajectoryPoint(560, 105.446119252229),
+    Boardcore::TrajectoryPoint(570, 104.20624602044),
+    Boardcore::TrajectoryPoint(580, 102.959006518586),
+    Boardcore::TrajectoryPoint(590, 101.70405989436),
+    Boardcore::TrajectoryPoint(600, 100.441050103474),
+    Boardcore::TrajectoryPoint(610, 99.1696051650051),
+    Boardcore::TrajectoryPoint(620, 97.8893363608352),
+    Boardcore::TrajectoryPoint(630, 96.5998373756894),
+    Boardcore::TrajectoryPoint(640, 95.3006833738967),
+    Boardcore::TrajectoryPoint(650, 93.9913215174958),
+    Boardcore::TrajectoryPoint(660, 92.6711599958102),
+    Boardcore::TrajectoryPoint(670, 91.3398601407874),
+    Boardcore::TrajectoryPoint(680, 89.9969054349139),
+    Boardcore::TrajectoryPoint(690, 88.6417546799249),
+    Boardcore::TrajectoryPoint(700, 87.2736284235172),
+    Boardcore::TrajectoryPoint(710, 85.8915342286298),
+    Boardcore::TrajectoryPoint(720, 84.495329045546),
+    Boardcore::TrajectoryPoint(730, 83.0839992592729),
+    Boardcore::TrajectoryPoint(740, 81.6563362139503),
+    Boardcore::TrajectoryPoint(750, 80.2122288237985),
+    Boardcore::TrajectoryPoint(760, 78.7495241096846),
+    Boardcore::TrajectoryPoint(770, 77.2683301540632),
+    Boardcore::TrajectoryPoint(780, 75.7665150159221),
+    Boardcore::TrajectoryPoint(790, 74.2432756148519),
+    Boardcore::TrajectoryPoint(800, 72.697364219074),
+    Boardcore::TrajectoryPoint(810, 71.1267035461265),
+    Boardcore::TrajectoryPoint(820, 69.5296178585841),
+    Boardcore::TrajectoryPoint(830, 67.904442903485),
+    Boardcore::TrajectoryPoint(840, 66.2489725744482),
+    Boardcore::TrajectoryPoint(850, 64.5607861529751),
+    Boardcore::TrajectoryPoint(860, 62.8372226895241),
+    Boardcore::TrajectoryPoint(870, 61.0751513796656),
+    Boardcore::TrajectoryPoint(880, 59.2704288396001),
+    Boardcore::TrajectoryPoint(890, 57.4200840825586),
+    Boardcore::TrajectoryPoint(900, 55.5185850593929),
+    Boardcore::TrajectoryPoint(910, 53.5613537488966),
+    Boardcore::TrajectoryPoint(920, 51.5404632028398),
+    Boardcore::TrajectoryPoint(930, 49.4491473518355),
+    Boardcore::TrajectoryPoint(940, 47.2779714775322),
+    Boardcore::TrajectoryPoint(950, 45.0144239949148),
+    Boardcore::TrajectoryPoint(960, 42.6446752670154),
+    Boardcore::TrajectoryPoint(970, 40.1490582186216),
+    Boardcore::TrajectoryPoint(980, 37.5031758068286),
+    Boardcore::TrajectoryPoint(990, 34.6716124620735),
+    Boardcore::TrajectoryPoint(1000, 31.6051090189613),
+    Boardcore::TrajectoryPoint(1010, 28.2277775815275),
+    Boardcore::TrajectoryPoint(1020, 24.4083375534924),
+    Boardcore::TrajectoryPoint(1030, 19.8973916846481),
+    Boardcore::TrajectoryPoint(1040, 14.0434058201059),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t2_open[] = {
+    Boardcore::TrajectoryPoint(0, 169.131483352568),
+    Boardcore::TrajectoryPoint(10, 168.021987670571),
+    Boardcore::TrajectoryPoint(20, 166.913723128917),
+    Boardcore::TrajectoryPoint(30, 165.806162864455),
+    Boardcore::TrajectoryPoint(40, 164.699223769936),
+    Boardcore::TrajectoryPoint(50, 163.592880806674),
+    Boardcore::TrajectoryPoint(60, 162.48696357211),
+    Boardcore::TrajectoryPoint(70, 161.381387135012),
+    Boardcore::TrajectoryPoint(80, 160.276067113742),
+    Boardcore::TrajectoryPoint(90, 159.170930177318),
+    Boardcore::TrajectoryPoint(100, 158.065865683118),
+    Boardcore::TrajectoryPoint(110, 156.960783273119),
+    Boardcore::TrajectoryPoint(120, 155.85560006476),
+    Boardcore::TrajectoryPoint(130, 154.75018605909),
+    Boardcore::TrajectoryPoint(140, 153.644479765301),
+    Boardcore::TrajectoryPoint(150, 152.538399670524),
+    Boardcore::TrajectoryPoint(160, 151.431797030004),
+    Boardcore::TrajectoryPoint(170, 150.32459554215),
+    Boardcore::TrajectoryPoint(180, 149.216746710064),
+    Boardcore::TrajectoryPoint(190, 148.108068726142),
+    Boardcore::TrajectoryPoint(200, 146.998489024823),
+    Boardcore::TrajectoryPoint(210, 145.887986867122),
+    Boardcore::TrajectoryPoint(220, 144.776291119778),
+    Boardcore::TrajectoryPoint(230, 143.663435860165),
+    Boardcore::TrajectoryPoint(240, 142.549316088821),
+    Boardcore::TrajectoryPoint(250, 141.433669120944),
+    Boardcore::TrajectoryPoint(260, 140.316623407473),
+    Boardcore::TrajectoryPoint(270, 139.197802610314),
+    Boardcore::TrajectoryPoint(280, 138.077314802393),
+    Boardcore::TrajectoryPoint(290, 136.954914511937),
+    Boardcore::TrajectoryPoint(300, 135.830521193842),
+    Boardcore::TrajectoryPoint(310, 134.704044358959),
+    Boardcore::TrajectoryPoint(320, 133.575269699904),
+    Boardcore::TrajectoryPoint(330, 132.444191668923),
+    Boardcore::TrajectoryPoint(340, 131.310546362978),
+    Boardcore::TrajectoryPoint(350, 130.174314121649),
+    Boardcore::TrajectoryPoint(360, 129.035294199772),
+    Boardcore::TrajectoryPoint(370, 127.893325305339),
+    Boardcore::TrajectoryPoint(380, 126.748410822322),
+    Boardcore::TrajectoryPoint(390, 125.600092047644),
+    Boardcore::TrajectoryPoint(400, 124.448626738249),
+    Boardcore::TrajectoryPoint(410, 123.293431340795),
+    Boardcore::TrajectoryPoint(420, 122.134621991213),
+    Boardcore::TrajectoryPoint(430, 120.972106862744),
+    Boardcore::TrajectoryPoint(440, 119.80525091659),
+    Boardcore::TrajectoryPoint(450, 118.634337515645),
+    Boardcore::TrajectoryPoint(460, 117.459166068322),
+    Boardcore::TrajectoryPoint(470, 116.279071370591),
+    Boardcore::TrajectoryPoint(480, 115.094276344341),
+    Boardcore::TrajectoryPoint(490, 113.904570619208),
+    Boardcore::TrajectoryPoint(500, 112.709651512399),
+    Boardcore::TrajectoryPoint(510, 111.509016602242),
+    Boardcore::TrajectoryPoint(520, 110.302745103897),
+    Boardcore::TrajectoryPoint(530, 109.090570767566),
+    Boardcore::TrajectoryPoint(540, 107.872215529525),
+    Boardcore::TrajectoryPoint(550, 106.647389030539),
+    Boardcore::TrajectoryPoint(560, 105.415723322119),
+    Boardcore::TrajectoryPoint(570, 104.17684569139),
+    Boardcore::TrajectoryPoint(580, 102.930585819697),
+    Boardcore::TrajectoryPoint(590, 101.676602868099),
+    Boardcore::TrajectoryPoint(600, 100.414540812681),
+    Boardcore::TrajectoryPoint(610, 99.1440276993012),
+    Boardcore::TrajectoryPoint(620, 97.8646748425003),
+    Boardcore::TrajectoryPoint(630, 96.5760759650707),
+    Boardcore::TrajectoryPoint(640, 95.2778062743917),
+    Boardcore::TrajectoryPoint(650, 93.9693465967026),
+    Boardcore::TrajectoryPoint(660, 92.6500370079844),
+    Boardcore::TrajectoryPoint(670, 91.3195733673837),
+    Boardcore::TrajectoryPoint(680, 89.9774392216188),
+    Boardcore::TrajectoryPoint(690, 88.623093439841),
+    Boardcore::TrajectoryPoint(700, 87.2557874432964),
+    Boardcore::TrajectoryPoint(710, 85.8744659108293),
+    Boardcore::TrajectoryPoint(720, 84.4790179690958),
+    Boardcore::TrajectoryPoint(730, 83.0684589788966),
+    Boardcore::TrajectoryPoint(740, 81.6415210126344),
+    Boardcore::TrajectoryPoint(750, 80.1981513291283),
+    Boardcore::TrajectoryPoint(760, 78.736139656041),
+    Boardcore::TrajectoryPoint(770, 77.2556500277865),
+    Boardcore::TrajectoryPoint(780, 75.7544960787138),
+    Boardcore::TrajectoryPoint(790, 74.2319274499954),
+    Boardcore::TrajectoryPoint(800, 72.6866698993925),
+    Boardcore::TrajectoryPoint(810, 71.1166221010087),
+    Boardcore::TrajectoryPoint(820, 69.5201565200394),
+    Boardcore::TrajectoryPoint(830, 67.8955845346031),
+    Boardcore::TrajectoryPoint(840, 66.2406999587355),
+    Boardcore::TrajectoryPoint(850, 64.5530820144768),
+    Boardcore::TrajectoryPoint(860, 62.8300697117241),
+    Boardcore::TrajectoryPoint(870, 61.0685513214076),
+    Boardcore::TrajectoryPoint(880, 59.2643440977881),
+    Boardcore::TrajectoryPoint(890, 57.4144972399486),
+    Boardcore::TrajectoryPoint(900, 55.5134951186859),
+    Boardcore::TrajectoryPoint(910, 53.5567414342047),
+    Boardcore::TrajectoryPoint(920, 51.5362937467672),
+    Boardcore::TrajectoryPoint(930, 49.4454170152926),
+    Boardcore::TrajectoryPoint(940, 47.2746604442557),
+    Boardcore::TrajectoryPoint(950, 45.0115245871944),
+    Boardcore::TrajectoryPoint(960, 42.6421539307963),
+    Boardcore::TrajectoryPoint(970, 40.1469050382637),
+    Boardcore::TrajectoryPoint(980, 37.5013681713287),
+    Boardcore::TrajectoryPoint(990, 34.6701360530485),
+    Boardcore::TrajectoryPoint(1000, 31.6039384468464),
+    Boardcore::TrajectoryPoint(1010, 28.2268936080629),
+    Boardcore::TrajectoryPoint(1020, 24.4077114236162),
+    Boardcore::TrajectoryPoint(1030, 19.8970027623163),
+    Boardcore::TrajectoryPoint(1040, 14.0432280242359),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t3_open[] = {
+    Boardcore::TrajectoryPoint(0, 169.018549440047),
+    Boardcore::TrajectoryPoint(10, 167.911215676248),
+    Boardcore::TrajectoryPoint(20, 166.804991979826),
+    Boardcore::TrajectoryPoint(30, 165.69944525064),
+    Boardcore::TrajectoryPoint(40, 164.594489580683),
+    Boardcore::TrajectoryPoint(50, 163.4900905106),
+    Boardcore::TrajectoryPoint(60, 162.386106556436),
+    Boardcore::TrajectoryPoint(70, 161.282438017206),
+    Boardcore::TrajectoryPoint(80, 160.179000977387),
+    Boardcore::TrajectoryPoint(90, 159.075717361825),
+    Boardcore::TrajectoryPoint(100, 157.972485314413),
+    Boardcore::TrajectoryPoint(110, 156.869211734768),
+    Boardcore::TrajectoryPoint(120, 155.765814158809),
+    Boardcore::TrajectoryPoint(130, 154.662167229108),
+    Boardcore::TrajectoryPoint(140, 153.558198856204),
+    Boardcore::TrajectoryPoint(150, 152.453834669369),
+    Boardcore::TrajectoryPoint(160, 151.34893883593),
+    Boardcore::TrajectoryPoint(170, 150.243407479148),
+    Boardcore::TrajectoryPoint(180, 149.137207868512),
+    Boardcore::TrajectoryPoint(190, 148.030176802289),
+    Boardcore::TrajectoryPoint(200, 146.92220241072),
+    Boardcore::TrajectoryPoint(210, 145.813285675937),
+    Boardcore::TrajectoryPoint(220, 144.703177666616),
+    Boardcore::TrajectoryPoint(230, 143.591865696552),
+    Boardcore::TrajectoryPoint(240, 142.479297525719),
+    Boardcore::TrajectoryPoint(250, 141.365152795154),
+    Boardcore::TrajectoryPoint(260, 140.249590977913),
+    Boardcore::TrajectoryPoint(270, 139.132263851042),
+    Boardcore::TrajectoryPoint(280, 138.013220513944),
+    Boardcore::TrajectoryPoint(290, 136.892278067816),
+    Boardcore::TrajectoryPoint(300, 135.769290791518),
+    Boardcore::TrajectoryPoint(310, 134.644236068848),
+    Boardcore::TrajectoryPoint(320, 133.516829985779),
+    Boardcore::TrajectoryPoint(330, 132.387138507394),
+    Boardcore::TrajectoryPoint(340, 131.254825239606),
+    Boardcore::TrajectoryPoint(350, 130.119944224944),
+    Boardcore::TrajectoryPoint(360, 128.982220695094),
+    Boardcore::TrajectoryPoint(370, 127.841567982095),
+    Boardcore::TrajectoryPoint(380, 126.697915100134),
+    Boardcore::TrajectoryPoint(390, 125.550877777939),
+    Boardcore::TrajectoryPoint(400, 124.400678193037),
+    Boardcore::TrajectoryPoint(410, 123.246691766367),
+    Boardcore::TrajectoryPoint(420, 122.089113737498),
+    Boardcore::TrajectoryPoint(430, 120.927774770742),
+    Boardcore::TrajectoryPoint(440, 119.762116130352),
+    Boardcore::TrajectoryPoint(450, 118.592384167447),
+    Boardcore::TrajectoryPoint(460, 117.418355128934),
+    Boardcore::TrajectoryPoint(470, 116.239392235664),
+    Boardcore::TrajectoryPoint(480, 115.055729180526),
+    Boardcore::TrajectoryPoint(490, 113.867139450122),
+    Boardcore::TrajectoryPoint(500, 112.673281335156),
+    Boardcore::TrajectoryPoint(510, 111.473729462122),
+    Boardcore::TrajectoryPoint(520, 110.268525081955),
+    Boardcore::TrajectoryPoint(530, 109.05740191317),
+    Boardcore::TrajectoryPoint(540, 107.840081871549),
+    Boardcore::TrajectoryPoint(550, 106.616274587488),
+    Boardcore::TrajectoryPoint(560, 105.38557467042),
+    Boardcore::TrajectoryPoint(570, 104.147684275799),
+    Boardcore::TrajectoryPoint(580, 102.902395813242),
+    Boardcore::TrajectoryPoint(590, 101.649368457204),
+    Boardcore::TrajectoryPoint(600, 100.388246204042),
+    Boardcore::TrajectoryPoint(610, 99.1186571261929),
+    Boardcore::TrajectoryPoint(620, 97.8402125705544),
+    Boardcore::TrajectoryPoint(630, 96.5525062975941),
+    Boardcore::TrajectoryPoint(640, 95.2551135572692),
+    Boardcore::TrajectoryPoint(650, 93.9475486582521),
+    Boardcore::TrajectoryPoint(660, 92.6290839281749),
+    Boardcore::TrajectoryPoint(670, 91.2994495693191),
+    Boardcore::TrajectoryPoint(680, 89.9581291918365),
+    Boardcore::TrajectoryPoint(690, 88.6045817314495),
+    Boardcore::TrajectoryPoint(700, 87.2380892989704),
+    Boardcore::TrajectoryPoint(710, 85.8575340628243),
+    Boardcore::TrajectoryPoint(720, 84.4628371342171),
+    Boardcore::TrajectoryPoint(730, 83.0530426691203),
+    Boardcore::TrajectoryPoint(740, 81.6268238372804),
+    Boardcore::TrajectoryPoint(750, 80.1841858723471),
+    Boardcore::TrajectoryPoint(760, 78.7228615773764),
+    Boardcore::TrajectoryPoint(770, 77.2430705742164),
+    Boardcore::TrajectoryPoint(780, 75.7425724303685),
+    Boardcore::TrajectoryPoint(790, 74.2206691589271),
+    Boardcore::TrajectoryPoint(800, 72.6760601813762),
+    Boardcore::TrajectoryPoint(810, 71.1066202938045),
+    Boardcore::TrajectoryPoint(820, 69.510769835374),
+    Boardcore::TrajectoryPoint(830, 67.886795980158),
+    Boardcore::TrajectoryPoint(840, 66.232492463208),
+    Boardcore::TrajectoryPoint(850, 64.5454384474621),
+    Boardcore::TrajectoryPoint(860, 62.8229729023937),
+    Boardcore::TrajectoryPoint(870, 61.0620030375042),
+    Boardcore::TrajectoryPoint(880, 59.2583070265182),
+    Boardcore::TrajectoryPoint(890, 57.4089541098089),
+    Boardcore::TrajectoryPoint(900, 55.508444958503),
+    Boardcore::TrajectoryPoint(910, 53.5521651256781),
+    Boardcore::TrajectoryPoint(920, 51.5321567947398),
+    Boardcore::TrajectoryPoint(930, 49.4417157242628),
+    Boardcore::TrajectoryPoint(940, 47.2713751593836),
+    Boardcore::TrajectoryPoint(950, 45.0086477015002),
+    Boardcore::TrajectoryPoint(960, 42.6396521535451),
+    Boardcore::TrajectoryPoint(970, 40.1447685402118),
+    Boardcore::TrajectoryPoint(980, 37.4995745225391),
+    Boardcore::TrajectoryPoint(990, 34.6686710532882),
+    Boardcore::TrajectoryPoint(1000, 31.6027769079599),
+    Boardcore::TrajectoryPoint(1010, 28.2260164462406),
+    Boardcore::TrajectoryPoint(1020, 24.4070901103829),
+    Boardcore::TrajectoryPoint(1030, 19.8966168260871),
+    Boardcore::TrajectoryPoint(1040, 14.043051589917),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t4_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.906587066945),
+    Boardcore::TrajectoryPoint(10, 167.801409690573),
+    Boardcore::TrajectoryPoint(20, 166.697207011095),
+    Boardcore::TrajectoryPoint(30, 165.593654322181),
+    Boardcore::TrajectoryPoint(40, 164.490666223961),
+    Boardcore::TrajectoryPoint(50, 163.388191156171),
+    Boardcore::TrajectoryPoint(60, 162.286121896822),
+    Boardcore::TrajectoryPoint(70, 161.184342976228),
+    Boardcore::TrajectoryPoint(80, 160.082770937899),
+    Boardcore::TrajectoryPoint(90, 158.981324724607),
+    Boardcore::TrajectoryPoint(100, 157.879907698388),
+    Boardcore::TrajectoryPoint(110, 156.778425804556),
+    Boardcore::TrajectoryPoint(120, 155.676796991135),
+    Boardcore::TrajectoryPoint(130, 154.574901930628),
+    Boardcore::TrajectoryPoint(140, 153.472655130933),
+    Boardcore::TrajectoryPoint(150, 152.36999076217),
+    Boardcore::TrajectoryPoint(160, 151.266787017324),
+    Boardcore::TrajectoryPoint(170, 150.162910201622),
+    Boardcore::TrajectoryPoint(180, 149.058344464392),
+    Boardcore::TrajectoryPoint(190, 147.952946108577),
+    Boardcore::TrajectoryPoint(200, 146.846562155861),
+    Boardcore::TrajectoryPoint(210, 145.739216200288),
+    Boardcore::TrajectoryPoint(220, 144.630682244579),
+    Boardcore::TrajectoryPoint(230, 143.520899375052),
+    Boardcore::TrajectoryPoint(240, 142.409869418917),
+    Boardcore::TrajectoryPoint(250, 141.297213178154),
+    Boardcore::TrajectoryPoint(260, 140.183121714668),
+    Boardcore::TrajectoryPoint(270, 139.067275405684),
+    Boardcore::TrajectoryPoint(280, 137.949663417069),
+    Boardcore::TrajectoryPoint(290, 136.830166276048),
+    Boardcore::TrajectoryPoint(300, 135.708572326618),
+    Boardcore::TrajectoryPoint(310, 134.584927495098),
+    Boardcore::TrajectoryPoint(320, 133.45887766802),
+    Boardcore::TrajectoryPoint(330, 132.330560842128),
+    Boardcore::TrajectoryPoint(340, 131.199567675304),
+    Boardcore::TrajectoryPoint(350, 130.066026309233),
+    Boardcore::TrajectoryPoint(360, 128.929587605292),
+    Boardcore::TrajectoryPoint(370, 127.790239817395),
+    Boardcore::TrajectoryPoint(380, 126.647837330158),
+    Boardcore::TrajectoryPoint(390, 125.502070524505),
+    Boardcore::TrajectoryPoint(400, 124.353125859225),
+    Boardcore::TrajectoryPoint(410, 123.200337733118),
+    Boardcore::TrajectoryPoint(420, 122.043980540315),
+    Boardcore::TrajectoryPoint(430, 120.883807398833),
+    Boardcore::TrajectoryPoint(440, 119.719335896335),
+    Boardcore::TrajectoryPoint(450, 118.550775340835),
+    Boardcore::TrajectoryPoint(460, 117.377862804209),
+    Boardcore::TrajectoryPoint(470, 116.20003806833),
+    Boardcore::TrajectoryPoint(480, 115.01749740655),
+    Boardcore::TrajectoryPoint(490, 113.830014234307),
+    Boardcore::TrajectoryPoint(500, 112.637207921443),
+    Boardcore::TrajectoryPoint(510, 111.438729956832),
+    Boardcore::TrajectoryPoint(520, 110.234583707746),
+    Boardcore::TrajectoryPoint(530, 109.024502861849),
+    Boardcore::TrajectoryPoint(540, 107.808209315058),
+    Boardcore::TrajectoryPoint(550, 106.585412687852),
+    Boardcore::TrajectoryPoint(560, 105.355670298971),
+    Boardcore::TrajectoryPoint(570, 104.11875887977),
+    Boardcore::TrajectoryPoint(580, 102.874433707719),
+    Boardcore::TrajectoryPoint(590, 101.622353970683),
+    Boardcore::TrajectoryPoint(600, 100.362163685198),
+    Boardcore::TrajectoryPoint(610, 99.0934909500701),
+    Boardcore::TrajectoryPoint(620, 97.8159471442535),
+    Boardcore::TrajectoryPoint(630, 96.5291260655025),
+    Boardcore::TrajectoryPoint(640, 95.2326030058845),
+    Boardcore::TrajectoryPoint(650, 93.9259255756011),
+    Boardcore::TrajectoryPoint(660, 92.6082987172587),
+    Boardcore::TrajectoryPoint(670, 91.2794867930224),
+    Boardcore::TrajectoryPoint(680, 89.9389734756901),
+    Boardcore::TrajectoryPoint(690, 88.5862177667234),
+    Boardcore::TrajectoryPoint(700, 87.220532283807),
+    Boardcore::TrajectoryPoint(710, 85.8407370560265),
+    Boardcore::TrajectoryPoint(720, 84.4467849886527),
+    Boardcore::TrajectoryPoint(730, 83.0377488536409),
+    Boardcore::TrajectoryPoint(740, 81.6122432842374),
+    Boardcore::TrajectoryPoint(750, 80.1703215296299),
+    Boardcore::TrajectoryPoint(760, 78.7096886114444),
+    Boardcore::TrajectoryPoint(770, 77.2305905999085),
+    Boardcore::TrajectoryPoint(780, 75.7307429428338),
+    Boardcore::TrajectoryPoint(790, 74.2094996787707),
+    Boardcore::TrajectoryPoint(800, 72.6655340655436),
+    Boardcore::TrajectoryPoint(810, 71.0966971850236),
+    Boardcore::TrajectoryPoint(820, 69.5014569248545),
+    Boardcore::TrajectoryPoint(830, 67.8780764183671),
+    Boardcore::TrajectoryPoint(840, 66.2243493222208),
+    Boardcore::TrajectoryPoint(850, 64.5378547406061),
+    Boardcore::TrajectoryPoint(860, 62.8159316027102),
+    Boardcore::TrajectoryPoint(870, 61.0555059212506),
+    Boardcore::TrajectoryPoint(880, 59.2523170678806),
+    Boardcore::TrajectoryPoint(890, 57.4034541812121),
+    Boardcore::TrajectoryPoint(900, 55.503434114371),
+    Boardcore::TrajectoryPoint(910, 53.5476244033823),
+    Boardcore::TrajectoryPoint(920, 51.5280519681874),
+    Boardcore::TrajectoryPoint(930, 49.4380431408588),
+    Boardcore::TrajectoryPoint(940, 47.2681153237535),
+    Boardcore::TrajectoryPoint(950, 45.0057930764432),
+    Boardcore::TrajectoryPoint(960, 42.6371697085639),
+    Boardcore::TrajectoryPoint(970, 40.1426485313466),
+    Boardcore::TrajectoryPoint(980, 37.4977946987562),
+    Boardcore::TrajectoryPoint(990, 34.667217331054),
+    Boardcore::TrajectoryPoint(1000, 31.6016242981433),
+    Boardcore::TrajectoryPoint(1010, 28.2251460176322),
+    Boardcore::TrajectoryPoint(1020, 24.4064735584277),
+    Boardcore::TrajectoryPoint(1030, 19.8962338417036),
+    Boardcore::TrajectoryPoint(1040, 14.04287650157),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t5_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.795584094774),
+    Boardcore::TrajectoryPoint(10, 167.692557307917),
+    Boardcore::TrajectoryPoint(20, 166.590356097278),
+    Boardcore::TrajectoryPoint(30, 165.488778228285),
+    Boardcore::TrajectoryPoint(40, 164.387738801583),
+    Boardcore::TrajectoryPoint(50, 163.287171349686),
+    Boardcore::TrajectoryPoint(60, 162.186998460698),
+    Boardcore::TrajectoryPoint(70, 161.08709113554),
+    Boardcore::TrajectoryPoint(80, 159.987366369802),
+    Boardcore::TrajectoryPoint(90, 158.88774182662),
+    Boardcore::TrajectoryPoint(100, 157.788122639031),
+    Boardcore::TrajectoryPoint(110, 156.688415524839),
+    Boardcore::TrajectoryPoint(120, 155.588538837923),
+    Boardcore::TrajectoryPoint(130, 154.488380621634),
+    Boardcore::TrajectoryPoint(140, 153.387839273574),
+    Boardcore::TrajectoryPoint(150, 152.286858854869),
+    Boardcore::TrajectoryPoint(160, 151.185332658691),
+    Boardcore::TrajectoryPoint(170, 150.083095008443),
+    Boardcore::TrajectoryPoint(180, 148.980148006983),
+    Boardcore::TrajectoryPoint(190, 147.876368328361),
+    Boardcore::TrajectoryPoint(200, 146.771560146817),
+    Boardcore::TrajectoryPoint(210, 145.665770526275),
+    Boardcore::TrajectoryPoint(220, 144.558797108522),
+    Boardcore::TrajectoryPoint(230, 143.450529343185),
+    Boardcore::TrajectoryPoint(240, 142.341005103139),
+    Boardcore::TrajectoryPoint(250, 141.229843069594),
+    Boardcore::TrajectoryPoint(260, 140.11720860009),
+    Boardcore::TrajectoryPoint(270, 139.00283041646),
+    Boardcore::TrajectoryPoint(280, 137.886636830349),
+    Boardcore::TrajectoryPoint(290, 136.76857261171),
+    Boardcore::TrajectoryPoint(300, 135.648359444435),
+    Boardcore::TrajectoryPoint(310, 134.526112435869),
+    Boardcore::TrajectoryPoint(320, 133.401406709066),
+    Boardcore::TrajectoryPoint(330, 132.274452784611),
+    Boardcore::TrajectoryPoint(340, 131.144767940103),
+    Boardcore::TrajectoryPoint(350, 130.012554789651),
+    Boardcore::TrajectoryPoint(360, 128.877389498505),
+    Boardcore::TrajectoryPoint(370, 127.739335520449),
+    Boardcore::TrajectoryPoint(380, 126.59817236926),
+    Boardcore::TrajectoryPoint(390, 125.453665281202),
+    Boardcore::TrajectoryPoint(400, 124.305964866261),
+    Boardcore::TrajectoryPoint(410, 123.154364510289),
+    Boardcore::TrajectoryPoint(420, 121.999217800315),
+    Boardcore::TrajectoryPoint(430, 120.840200282556),
+    Boardcore::TrajectoryPoint(440, 119.67690587732),
+    Boardcore::TrajectoryPoint(450, 118.509506824211),
+    Boardcore::TrajectoryPoint(460, 117.337701080435),
+    Boardcore::TrajectoryPoint(470, 116.161004906083),
+    Boardcore::TrajectoryPoint(480, 114.979577179653),
+    Boardcore::TrajectoryPoint(490, 113.793191246996),
+    Boardcore::TrajectoryPoint(500, 112.601427664848),
+    Boardcore::TrajectoryPoint(510, 111.404014593817),
+    Boardcore::TrajectoryPoint(520, 110.200917600784),
+    Boardcore::TrajectoryPoint(530, 108.99187034337),
+    Boardcore::TrajectoryPoint(540, 107.77659469824),
+    Boardcore::TrajectoryPoint(550, 106.554800276382),
+    Boardcore::TrajectoryPoint(560, 105.326007257754),
+    Boardcore::TrajectoryPoint(570, 104.090066655839),
+    Boardcore::TrajectoryPoint(580, 102.846696756374),
+    Boardcore::TrajectoryPoint(590, 101.595556760649),
+    Boardcore::TrajectoryPoint(600, 100.336290705274),
+    Boardcore::TrajectoryPoint(610, 99.0685267152186),
+    Boardcore::TrajectoryPoint(620, 97.7918762011947),
+    Boardcore::TrajectoryPoint(630, 96.5059329978569),
+    Boardcore::TrajectoryPoint(640, 95.2102724389206),
+    Boardcore::TrajectoryPoint(650, 93.9044503648583),
+    Boardcore::TrajectoryPoint(660, 92.5876793685652),
+    Boardcore::TrajectoryPoint(670, 91.2596831159777),
+    Boardcore::TrajectoryPoint(680, 89.9199702329934),
+    Boardcore::TrajectoryPoint(690, 88.5679997859932),
+    Boardcore::TrajectoryPoint(700, 87.2031147181269),
+    Boardcore::TrajectoryPoint(710, 85.8240732876301),
+    Boardcore::TrajectoryPoint(720, 84.4308600046887),
+    Boardcore::TrajectoryPoint(730, 83.0225760794811),
+    Boardcore::TrajectoryPoint(740, 81.5977779720038),
+    Boardcore::TrajectoryPoint(750, 80.1565488685423),
+    Boardcore::TrajectoryPoint(760, 78.6966195158749),
+    Boardcore::TrajectoryPoint(770, 77.2182089301946),
+    Boardcore::TrajectoryPoint(780, 75.7190065057806),
+    Boardcore::TrajectoryPoint(790, 74.1984179633334),
+    Boardcore::TrajectoryPoint(800, 72.6550905680859),
+    Boardcore::TrajectoryPoint(810, 71.0868518498875),
+    Boardcore::TrajectoryPoint(820, 69.4922169225084),
+    Boardcore::TrajectoryPoint(830, 67.869425040289),
+    Boardcore::TrajectoryPoint(840, 66.2162697820797),
+    Boardcore::TrajectoryPoint(850, 64.530330193674),
+    Boardcore::TrajectoryPoint(860, 62.8089451641102),
+    Boardcore::TrajectoryPoint(870, 61.0490593753814),
+    Boardcore::TrajectoryPoint(880, 59.2463736726344),
+    Boardcore::TrajectoryPoint(890, 57.3979969511595),
+    Boardcore::TrajectoryPoint(900, 55.4984621290173),
+    Boardcore::TrajectoryPoint(910, 53.5431082425134),
+    Boardcore::TrajectoryPoint(920, 51.5239788943938),
+    Boardcore::TrajectoryPoint(930, 49.4343989324123),
+    Boardcore::TrajectoryPoint(940, 47.2648806428178),
+    Boardcore::TrajectoryPoint(950, 45.0029604546617),
+    Boardcore::TrajectoryPoint(960, 42.6347063726429),
+    Boardcore::TrajectoryPoint(970, 40.1405448215165),
+    Boardcore::TrajectoryPoint(980, 37.4960285407582),
+    Boardcore::TrajectoryPoint(990, 34.6657747566259),
+    Boardcore::TrajectoryPoint(1000, 31.6004805148312),
+    Boardcore::TrajectoryPoint(1010, 28.2242822450064),
+    Boardcore::TrajectoryPoint(1020, 24.405861713229),
+    Boardcore::TrajectoryPoint(1030, 19.8958537754282),
+    Boardcore::TrajectoryPoint(1040, 14.0427027438515),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t6_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.685570230743),
+    Boardcore::TrajectoryPoint(10, 167.584646329466),
+    Boardcore::TrajectoryPoint(20, 166.484427314696),
+    Boardcore::TrajectoryPoint(30, 165.384805314988),
+    Boardcore::TrajectoryPoint(40, 164.28569592402),
+    Boardcore::TrajectoryPoint(50, 163.187019887333),
+    Boardcore::TrajectoryPoint(60, 162.088725300665),
+    Boardcore::TrajectoryPoint(70, 160.990671799155),
+    Boardcore::TrajectoryPoint(80, 159.892776823657),
+    Boardcore::TrajectoryPoint(90, 158.794958276955),
+    Boardcore::TrajectoryPoint(100, 157.697120109588),
+    Boardcore::TrajectoryPoint(110, 156.599171102938),
+    Boardcore::TrajectoryPoint(120, 155.501030136122),
+    Boardcore::TrajectoryPoint(130, 154.402593918367),
+    Boardcore::TrajectoryPoint(140, 153.303742122402),
+    Boardcore::TrajectoryPoint(150, 152.204430003618),
+    Boardcore::TrajectoryPoint(160, 151.104566992153),
+    Boardcore::TrajectoryPoint(170, 150.003953342255),
+    Boardcore::TrajectoryPoint(180, 148.902610145574),
+    Boardcore::TrajectoryPoint(190, 147.800435282399),
+    Boardcore::TrajectoryPoint(200, 146.697188403928),
+    Boardcore::TrajectoryPoint(210, 145.592940870211),
+    Boardcore::TrajectoryPoint(220, 144.48751464093),
+    Boardcore::TrajectoryPoint(230, 143.380748172669),
+    Boardcore::TrajectoryPoint(240, 142.272706918421),
+    Boardcore::TrajectoryPoint(250, 141.163035387437),
+    Boardcore::TrajectoryPoint(260, 140.051844731605),
+    Boardcore::TrajectoryPoint(270, 138.938922138147),
+    Boardcore::TrajectoryPoint(280, 137.824134181808),
+    Boardcore::TrajectoryPoint(290, 136.707490656827),
+    Boardcore::TrajectoryPoint(300, 135.588645894221),
+    Boardcore::TrajectoryPoint(310, 134.467784790821),
+    Boardcore::TrajectoryPoint(320, 133.34441116998),
+    Boardcore::TrajectoryPoint(330, 132.218808542547),
+    Boardcore::TrajectoryPoint(340, 131.090420397478),
+    Boardcore::TrajectoryPoint(350, 129.959524172423),
+    Boardcore::TrajectoryPoint(360, 128.825621031295),
+    Boardcore::TrajectoryPoint(370, 127.6888498866),
+    Boardcore::TrajectoryPoint(380, 126.548915157874),
+    Boardcore::TrajectoryPoint(390, 125.405657123228),
+    Boardcore::TrajectoryPoint(400, 124.259156659437),
+    Boardcore::TrajectoryPoint(410, 123.10876744383),
+    Boardcore::TrajectoryPoint(420, 121.954820992701),
+    Boardcore::TrajectoryPoint(430, 120.796949029684),
+    Boardcore::TrajectoryPoint(440, 119.63482180624),
+    Boardcore::TrajectoryPoint(450, 118.468574474068),
+    Boardcore::TrajectoryPoint(460, 117.297865939559),
+    Boardcore::TrajectoryPoint(470, 116.122288850337),
+    Boardcore::TrajectoryPoint(480, 114.941964719029),
+    Boardcore::TrajectoryPoint(490, 113.756666823441),
+    Boardcore::TrajectoryPoint(500, 112.565937016961),
+    Boardcore::TrajectoryPoint(510, 111.369579936659),
+    Boardcore::TrajectoryPoint(520, 110.16752343488),
+    Boardcore::TrajectoryPoint(530, 108.959501139991),
+    Boardcore::TrajectoryPoint(540, 107.745234909991),
+    Boardcore::TrajectoryPoint(550, 106.524434346794),
+    Boardcore::TrajectoryPoint(560, 105.296582643942),
+    Boardcore::TrajectoryPoint(570, 104.061604802052),
+    Boardcore::TrajectoryPoint(580, 102.819182256317),
+    Boardcore::TrajectoryPoint(590, 101.568974221457),
+    Boardcore::TrajectoryPoint(600, 100.310624754052),
+    Boardcore::TrajectoryPoint(610, 99.043762005029),
+    Boardcore::TrajectoryPoint(620, 97.7679974165565),
+    Boardcore::TrajectoryPoint(630, 96.4829248598084),
+    Boardcore::TrajectoryPoint(640, 95.1881197096899),
+    Boardcore::TrajectoryPoint(650, 93.8831378521849),
+    Boardcore::TrajectoryPoint(660, 92.5672239072352),
+    Boardcore::TrajectoryPoint(670, 91.2400366461118),
+    Boardcore::TrajectoryPoint(680, 89.9011176526654),
+    Boardcore::TrajectoryPoint(690, 88.5499260573886),
+    Boardcore::TrajectoryPoint(700, 87.1858349487717),
+    Boardcore::TrajectoryPoint(710, 85.8075411801055),
+    Boardcore::TrajectoryPoint(720, 84.4150606786734),
+    Boardcore::TrajectoryPoint(730, 83.007522916533),
+    Boardcore::TrajectoryPoint(740, 81.5834265407946),
+    Boardcore::TrajectoryPoint(750, 80.1428844882167),
+    Boardcore::TrajectoryPoint(760, 78.6836530677857),
+    Boardcore::TrajectoryPoint(770, 77.205924408817),
+    Boardcore::TrajectoryPoint(780, 75.7073620262579),
+    Boardcore::TrajectoryPoint(790, 74.1874229827818),
+    Boardcore::TrajectoryPoint(800, 72.6447287205635),
+    Boardcore::TrajectoryPoint(810, 71.0770833780436),
+    Boardcore::TrajectoryPoint(820, 69.4830489758586),
+    Boardcore::TrajectoryPoint(830, 67.8608410495752),
+    Boardcore::TrajectoryPoint(840, 66.2082531008103),
+    Boardcore::TrajectoryPoint(850, 64.5228641173075),
+    Boardcore::TrajectoryPoint(860, 62.8020129480924),
+    Boardcore::TrajectoryPoint(870, 61.0426628118896),
+    Boardcore::TrajectoryPoint(880, 59.2404763000421),
+    Boardcore::TrajectoryPoint(890, 57.39258192443),
+    Boardcore::TrajectoryPoint(900, 55.4935285522326),
+    Boardcore::TrajectoryPoint(910, 53.5386220306471),
+    Boardcore::TrajectoryPoint(920, 51.5199372063863),
+    Boardcore::TrajectoryPoint(930, 49.4307827713754),
+    Boardcore::TrajectoryPoint(940, 47.2616708265571),
+    Boardcore::TrajectoryPoint(950, 45.000149582747),
+    Boardcore::TrajectoryPoint(960, 42.6322619259962),
+    Boardcore::TrajectoryPoint(970, 40.1384572234834),
+    Boardcore::TrajectoryPoint(980, 37.4942758917594),
+    Boardcore::TrajectoryPoint(990, 34.6643432022668),
+    Boardcore::TrajectoryPoint(1000, 31.599345457024),
+    Boardcore::TrajectoryPoint(1010, 28.2234250523099),
+    Boardcore::TrajectoryPoint(1020, 24.4052545210955),
+    Boardcore::TrajectoryPoint(1030, 19.8954765940369),
+    Boardcore::TrajectoryPoint(1040, 14.0425303016496),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t7_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.576758674548),
+    Boardcore::TrajectoryPoint(10, 167.477664759087),
+    Boardcore::TrajectoryPoint(20, 166.379408937412),
+    Boardcore::TrajectoryPoint(30, 165.281724121234),
+    Boardcore::TrajectoryPoint(40, 164.184526389915),
+    Boardcore::TrajectoryPoint(50, 163.087732411267),
+    Boardcore::TrajectoryPoint(60, 161.991291650769),
+    Boardcore::TrajectoryPoint(70, 160.895074448018),
+    Boardcore::TrajectoryPoint(80, 159.798992022537),
+    Boardcore::TrajectoryPoint(90, 158.702962703546),
+    Boardcore::TrajectoryPoint(100, 157.606890249155),
+    Boardcore::TrajectoryPoint(110, 156.510682907825),
+    Boardcore::TrajectoryPoint(120, 155.414261480221),
+    Boardcore::TrajectoryPoint(130, 154.317532592117),
+    Boardcore::TrajectoryPoint(140, 153.220354666761),
+    Boardcore::TrajectoryPoint(150, 152.122695411745),
+    Boardcore::TrajectoryPoint(160, 151.02447466393),
+    Boardcore::TrajectoryPoint(170, 149.925476786568),
+    Boardcore::TrajectoryPoint(180, 148.825722666637),
+    Boardcore::TrajectoryPoint(190, 147.725138906152),
+    Boardcore::TrajectoryPoint(200, 146.623439078599),
+    Boardcore::TrajectoryPoint(210, 145.520719575995),
+    Boardcore::TrajectoryPoint(220, 144.416827349337),
+    Boardcore::TrajectoryPoint(230, 143.311548556918),
+    Boardcore::TrajectoryPoint(240, 142.204976870697),
+    Boardcore::TrajectoryPoint(250, 141.096783165562),
+    Boardcore::TrajectoryPoint(260, 139.987023319395),
+    Boardcore::TrajectoryPoint(270, 138.875543935805),
+    Boardcore::TrajectoryPoint(280, 137.762149006706),
+    Boardcore::TrajectoryPoint(290, 136.646914098217),
+    Boardcore::TrajectoryPoint(300, 135.529425527086),
+    Boardcore::TrajectoryPoint(310, 134.409938559071),
+    Boardcore::TrajectoryPoint(320, 133.287885208454),
+    Boardcore::TrajectoryPoint(330, 132.163622417914),
+    Boardcore::TrajectoryPoint(340, 131.036519502466),
+    Boardcore::TrajectoryPoint(350, 129.906929053034),
+    Boardcore::TrajectoryPoint(360, 128.774276946874),
+    Boardcore::TrajectoryPoint(370, 127.638777795595),
+    Boardcore::TrajectoryPoint(380, 126.500060718327),
+    Boardcore::TrajectoryPoint(390, 125.358041205484),
+    Boardcore::TrajectoryPoint(400, 124.212725786401),
+    Boardcore::TrajectoryPoint(410, 123.063541954857),
+    Boardcore::TrajectoryPoint(420, 121.910785665741),
+    Boardcore::TrajectoryPoint(430, 120.754049318777),
+    Boardcore::TrajectoryPoint(440, 119.593079484775),
+    Boardcore::TrajectoryPoint(450, 118.42797421363),
+    Boardcore::TrajectoryPoint(460, 117.258353428119),
+    Boardcore::TrajectoryPoint(470, 116.083886065141),
+    Boardcore::TrajectoryPoint(480, 114.904656304586),
+    Boardcore::TrajectoryPoint(490, 113.720437357707),
+    Boardcore::TrajectoryPoint(500, 112.530732486217),
+    Boardcore::TrajectoryPoint(510, 111.335422603955),
+    Boardcore::TrajectoryPoint(520, 110.134397937061),
+    Boardcore::TrajectoryPoint(530, 108.927392085409),
+    Boardcore::TrajectoryPoint(540, 107.714126888904),
+    Boardcore::TrajectoryPoint(550, 106.494282983142),
+    Boardcore::TrajectoryPoint(560, 105.267393600956),
+    Boardcore::TrajectoryPoint(570, 104.03337056106),
+    Boardcore::TrajectoryPoint(580, 102.791887547647),
+    Boardcore::TrajectoryPoint(590, 101.54260378887),
+    Boardcore::TrajectoryPoint(600, 100.28516336117),
+    Boardcore::TrajectoryPoint(610, 99.0191944412223),
+    Boardcore::TrajectoryPoint(620, 97.7443085023561),
+    Boardcore::TrajectoryPoint(630, 96.4600994518848),
+    Boardcore::TrajectoryPoint(640, 95.1661427054507),
+    Boardcore::TrajectoryPoint(650, 93.8619941940291),
+    Boardcore::TrajectoryPoint(660, 92.546930389593),
+    Boardcore::TrajectoryPoint(670, 91.2205455211946),
+    Boardcore::TrajectoryPoint(680, 89.8824139521582),
+    Boardcore::TrajectoryPoint(690, 88.5319948762927),
+    Boardcore::TrajectoryPoint(700, 87.1686913485823),
+    Boardcore::TrajectoryPoint(710, 85.7911391807029),
+    Boardcore::TrajectoryPoint(720, 84.3993855305449),
+    Boardcore::TrajectoryPoint(730, 82.9925879571092),
+    Boardcore::TrajectoryPoint(740, 81.5691876521153),
+    Boardcore::TrajectoryPoint(750, 80.1293271176714),
+    Boardcore::TrajectoryPoint(760, 78.6707880634015),
+    Boardcore::TrajectoryPoint(770, 77.1937358975682),
+    Boardcore::TrajectoryPoint(780, 75.6958084283538),
+    Boardcore::TrajectoryPoint(790, 74.1765137233225),
+    Boardcore::TrajectoryPoint(800, 72.6344475696059),
+    Boardcore::TrajectoryPoint(810, 71.067390873285),
+    Boardcore::TrajectoryPoint(820, 69.4739522456603),
+    Boardcore::TrajectoryPoint(830, 67.8523236622251),
+    Boardcore::TrajectoryPoint(840, 66.2002985479306),
+    Boardcore::TrajectoryPoint(850, 64.5154558328133),
+    Boardcore::TrajectoryPoint(860, 62.7951343260225),
+    Boardcore::TrajectoryPoint(870, 61.0363156518462),
+    Boardcore::TrajectoryPoint(880, 59.2346244177048),
+    Boardcore::TrajectoryPoint(890, 57.3872086134296),
+    Boardcore::TrajectoryPoint(900, 55.4886329407341),
+    Boardcore::TrajectoryPoint(910, 53.5341702952857),
+    Boardcore::TrajectoryPoint(920, 51.5159265428244),
+    Boardcore::TrajectoryPoint(930, 49.4271943352214),
+    Boardcore::TrajectoryPoint(940, 47.2584855893927),
+    Boardcore::TrajectoryPoint(950, 44.9973602111657),
+    Boardcore::TrajectoryPoint(960, 42.6298361521946),
+    Boardcore::TrajectoryPoint(970, 40.1363855528654),
+    Boardcore::TrajectoryPoint(980, 37.4925365973632),
+    Boardcore::TrajectoryPoint(990, 34.6629225421831),
+    Boardcore::TrajectoryPoint(1000, 31.598219025257),
+    Boardcore::TrajectoryPoint(1010, 28.222574364643),
+    Boardcore::TrajectoryPoint(1020, 24.404651929149),
+    Boardcore::TrajectoryPoint(1030, 19.8951022648077),
+    Boardcore::TrajectoryPoint(1040, 14.042359160082),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t8_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.46886387969),
+    Boardcore::TrajectoryPoint(10, 167.371600799283),
+    Boardcore::TrajectoryPoint(20, 166.275289433288),
+    Boardcore::TrajectoryPoint(30, 165.179523375033),
+    Boardcore::TrajectoryPoint(40, 164.084219182355),
+    Boardcore::TrajectoryPoint(50, 162.989293988637),
+    Boardcore::TrajectoryPoint(60, 161.894686922867),
+    Boardcore::TrajectoryPoint(70, 160.800288736459),
+    Boardcore::TrajectoryPoint(80, 159.706001858579),
+    Boardcore::TrajectoryPoint(90, 158.611745038509),
+    Boardcore::TrajectoryPoint(100, 157.517423359333),
+    Boardcore::TrajectoryPoint(110, 156.422941466866),
+    Boardcore::TrajectoryPoint(120, 155.328223619085),
+    Boardcore::TrajectoryPoint(130, 154.233187566099),
+    Boardcore::TrajectoryPoint(140, 153.13766804403),
+    Boardcore::TrajectoryPoint(150, 152.041646426804),
+    Boardcore::TrajectoryPoint(160, 150.945042898062),
+    Boardcore::TrajectoryPoint(170, 149.847657062916),
+    Boardcore::TrajectoryPoint(180, 148.749477491067),
+    Boardcore::TrajectoryPoint(190, 147.650448858313),
+    Boardcore::TrajectoryPoint(200, 146.550304450661),
+    Boardcore::TrajectoryPoint(210, 145.449099112549),
+    Boardcore::TrajectoryPoint(220, 144.346727863797),
+    Boardcore::TrajectoryPoint(230, 143.242923308588),
+    Boardcore::TrajectoryPoint(240, 142.137807951998),
+    Boardcore::TrajectoryPoint(250, 141.031079551436),
+    Boardcore::TrajectoryPoint(260, 139.922737684132),
+    Boardcore::TrajectoryPoint(270, 138.812689282553),
+    Boardcore::TrajectoryPoint(280, 137.700674945383),
+    Boardcore::TrajectoryPoint(290, 136.586836725379),
+    Boardcore::TrajectoryPoint(300, 135.470692293954),
+    Boardcore::TrajectoryPoint(310, 134.352557901333),
+    Boardcore::TrajectoryPoint(320, 133.231823076879),
+    Boardcore::TrajectoryPoint(330, 132.108880822872),
+    Boardcore::TrajectoryPoint(340, 130.983059799829),
+    Boardcore::TrajectoryPoint(350, 129.854764114432),
+    Boardcore::TrajectoryPoint(360, 128.723352073366),
+    Boardcore::TrajectoryPoint(370, 127.589114209884),
+    Boardcore::TrajectoryPoint(380, 126.451604153197),
+    Boardcore::TrajectoryPoint(390, 125.310812760981),
+    Boardcore::TrajectoryPoint(400, 124.166672084618),
+    Boardcore::TrajectoryPoint(410, 123.018683538149),
+    Boardcore::TrajectoryPoint(420, 121.867107439301),
+    Boardcore::TrajectoryPoint(430, 120.711496897768),
+    Boardcore::TrajectoryPoint(440, 119.55167478198),
+    Boardcore::TrajectoryPoint(450, 118.387702031519),
+    Boardcore::TrajectoryPoint(460, 117.219159655951),
+    Boardcore::TrajectoryPoint(470, 116.045792775939),
+    Boardcore::TrajectoryPoint(480, 114.86764827574),
+    Boardcore::TrajectoryPoint(490, 113.684499301507),
+    Boardcore::TrajectoryPoint(500, 112.495810636763),
+    Boardcore::TrajectoryPoint(510, 111.301539268224),
+    Boardcore::TrajectoryPoint(520, 110.101537886512),
+    Boardcore::TrajectoryPoint(530, 108.895540063746),
+    Boardcore::TrajectoryPoint(540, 107.683267622292),
+    Boardcore::TrajectoryPoint(550, 106.464364616841),
+    Boardcore::TrajectoryPoint(560, 105.238437317554),
+    Boardcore::TrajectoryPoint(570, 104.005361219239),
+    Boardcore::TrajectoryPoint(580, 102.764810012608),
+    Boardcore::TrajectoryPoint(590, 101.516442939243),
+    Boardcore::TrajectoryPoint(600, 100.259904095332),
+    Boardcore::TrajectoryPoint(610, 98.9948216830954),
+    Boardcore::TrajectoryPoint(620, 97.7208072067237),
+    Boardcore::TrajectoryPoint(630, 96.4374546092949),
+    Boardcore::TrajectoryPoint(640, 95.1443393467403),
+    Boardcore::TrajectoryPoint(650, 93.8410173945125),
+    Boardcore::TrajectoryPoint(660, 92.5267969025352),
+    Boardcore::TrajectoryPoint(670, 91.201207908254),
+    Boardcore::TrajectoryPoint(680, 89.8638573768977),
+    Boardcore::TrajectoryPoint(690, 88.5142045648081),
+    Boardcore::TrajectoryPoint(700, 87.1516823158908),
+    Boardcore::TrajectoryPoint(710, 85.7748657609686),
+    Boardcore::TrajectoryPoint(720, 84.3838331033719),
+    Boardcore::TrajectoryPoint(730, 82.977769815506),
+    Boardcore::TrajectoryPoint(740, 81.555059988348),
+    Boardcore::TrajectoryPoint(750, 80.1158755057223),
+    Boardcore::TrajectoryPoint(760, 78.658023317683),
+    Boardcore::TrajectoryPoint(770, 77.181642275941),
+    Boardcore::TrajectoryPoint(780, 75.6843446528651),
+    Boardcore::TrajectoryPoint(790, 74.1656891868915),
+    Boardcore::TrajectoryPoint(800, 72.6242461766202),
+    Boardcore::TrajectoryPoint(810, 71.0577734532764),
+    Boardcore::TrajectoryPoint(820, 69.4649259056458),
+    Boardcore::TrajectoryPoint(830, 67.843872106348),
+    Boardcore::TrajectoryPoint(840, 66.1924054042294),
+    Boardcore::TrajectoryPoint(850, 64.5081046719584),
+    Boardcore::TrajectoryPoint(860, 62.7883086789434),
+    Boardcore::TrajectoryPoint(870, 61.0300173252268),
+    Boardcore::TrajectoryPoint(880, 59.2288175014017),
+    Boardcore::TrajectoryPoint(890, 57.381876538045),
+    Boardcore::TrajectoryPoint(900, 55.4837748580334),
+    Boardcore::TrajectoryPoint(910, 53.5297526405762),
+    Boardcore::TrajectoryPoint(920, 51.5119465478925),
+    Boardcore::TrajectoryPoint(930, 49.4236333063493),
+    Boardcore::TrajectoryPoint(940, 47.255324650102),
+    Boardcore::TrajectoryPoint(950, 44.9945920941872),
+    Boardcore::TrajectoryPoint(960, 42.6274288381029),
+    Boardcore::TrajectoryPoint(970, 40.1343296280831),
+    Boardcore::TrajectoryPoint(980, 37.4908105055163),
+    Boardcore::TrajectoryPoint(990, 34.6615126524883),
+    Boardcore::TrajectoryPoint(1000, 31.5971011215711),
+    Boardcore::TrajectoryPoint(1010, 28.2217301082383),
+    Boardcore::TrajectoryPoint(1020, 24.4040538853095),
+    Boardcore::TrajectoryPoint(1030, 19.8947307555111),
+    Boardcore::TrajectoryPoint(1040, 14.0421893044884),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t9_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.361874651667),
+    Boardcore::TrajectoryPoint(10, 167.266442847243),
+    Boardcore::TrajectoryPoint(20, 166.172057460137),
+    Boardcore::TrajectoryPoint(30, 165.078191989716),
+    Boardcore::TrajectoryPoint(40, 163.984763465206),
+    Boardcore::TrajectoryPoint(50, 162.891689478684),
+    Boardcore::TrajectoryPoint(60, 161.798900703067),
+    Boardcore::TrajectoryPoint(70, 160.706304488736),
+    Boardcore::TrajectoryPoint(80, 159.613796389609),
+    Boardcore::TrajectoryPoint(90, 158.521295568787),
+    Boardcore::TrajectoryPoint(100, 157.428709900975),
+    Boardcore::TrajectoryPoint(110, 156.335937462659),
+    Boardcore::TrajectoryPoint(120, 155.242907452877),
+    Boardcore::TrajectoryPoint(130, 154.149540072807),
+    Boardcore::TrajectoryPoint(140, 153.055673536648),
+    Boardcore::TrajectoryPoint(150, 151.961274537679),
+    Boardcore::TrajectoryPoint(160, 150.866273440767),
+    Boardcore::TrajectoryPoint(170, 149.770486028088),
+    Boardcore::TrajectoryPoint(180, 148.673866671489),
+    Boardcore::TrajectoryPoint(190, 147.576379051883),
+    Boardcore::TrajectoryPoint(200, 146.477776925786),
+    Boardcore::TrajectoryPoint(210, 145.378072071307),
+    Boardcore::TrajectoryPoint(220, 144.277208934424),
+    Boardcore::TrajectoryPoint(230, 143.174865357185),
+    Boardcore::TrajectoryPoint(240, 142.071193268059),
+    Boardcore::TrajectoryPoint(250, 140.965917803829),
+    Boardcore::TrajectoryPoint(260, 139.858981254763),
+    Boardcore::TrajectoryPoint(270, 138.750351757402),
+    Boardcore::TrajectoryPoint(280, 137.639705741156),
+    Boardcore::TrajectoryPoint(290, 136.527252428432),
+    Boardcore::TrajectoryPoint(300, 135.412440243561),
+    Boardcore::TrajectoryPoint(310, 134.295622180286),
+    Boardcore::TrajectoryPoint(320, 133.176219120437),
+    Boardcore::TrajectoryPoint(330, 132.054558471532),
+    Boardcore::TrajectoryPoint(340, 130.930035922262),
+    Boardcore::TrajectoryPoint(350, 129.803024125281),
+    Boardcore::TrajectoryPoint(360, 128.672841322107),
+    Boardcore::TrajectoryPoint(370, 127.539854172974),
+    Boardcore::TrajectoryPoint(380, 126.40354064371),
+    Boardcore::TrajectoryPoint(390, 125.263967099277),
+    Boardcore::TrajectoryPoint(400, 124.120990996584),
+    Boardcore::TrajectoryPoint(410, 122.974187760683),
+    Boardcore::TrajectoryPoint(420, 121.823782003417),
+    Boardcore::TrajectoryPoint(430, 120.669287582584),
+    Boardcore::TrajectoryPoint(440, 119.510603632944),
+    Boardcore::TrajectoryPoint(450, 118.347753980452),
+    Boardcore::TrajectoryPoint(460, 117.180280794938),
+    Boardcore::TrajectoryPoint(470, 116.008005268345),
+    Boardcore::TrajectoryPoint(480, 114.830937030228),
+    Boardcore::TrajectoryPoint(490, 113.648849163055),
+    Boardcore::TrajectoryPoint(500, 112.46116808736),
+    Boardcore::TrajectoryPoint(510, 111.267926654841),
+    Boardcore::TrajectoryPoint(520, 110.068940113543),
+    Boardcore::TrajectoryPoint(530, 108.863942008548),
+    Boardcore::TrajectoryPoint(540, 107.652654145213),
+    Boardcore::TrajectoryPoint(550, 106.434684140738),
+    Boardcore::TrajectoryPoint(560, 105.209711026935),
+    Boardcore::TrajectoryPoint(570, 103.977574105828),
+    Boardcore::TrajectoryPoint(580, 102.737947074756),
+    Boardcore::TrajectoryPoint(590, 101.49048918872),
+    Boardcore::TrajectoryPoint(600, 100.234844563544),
+    Boardcore::TrajectoryPoint(610, 98.9706414267826),
+    Boardcore::TrajectoryPoint(620, 97.6974913131941),
+    Boardcore::TrajectoryPoint(630, 96.4149882012485),
+    Boardcore::TrajectoryPoint(640, 95.1227075867228),
+    Boardcore::TrajectoryPoint(650, 93.8202054890391),
+    Boardcore::TrajectoryPoint(660, 92.5068215629322),
+    Boardcore::TrajectoryPoint(670, 91.1820220030042),
+    Boardcore::TrajectoryPoint(680, 89.845446199738),
+    Boardcore::TrajectoryPoint(690, 88.4965534712364),
+    Boardcore::TrajectoryPoint(700, 87.1347770292141),
+    Boardcore::TrajectoryPoint(710, 85.7587194162718),
+    Boardcore::TrajectoryPoint(720, 84.3684019629031),
+    Boardcore::TrajectoryPoint(730, 82.9630671275761),
+    Boardcore::TrajectoryPoint(740, 81.5410422523463),
+    Boardcore::TrajectoryPoint(750, 80.1025284205998),
+    Boardcore::TrajectoryPoint(760, 78.6453576639641),
+    Boardcore::TrajectoryPoint(770, 77.1696424407859),
+    Boardcore::TrajectoryPoint(780, 75.6729696569747),
+    Boardcore::TrajectoryPoint(790, 74.154948390851),
+    Boardcore::TrajectoryPoint(800, 72.6141236175065),
+    Boardcore::TrajectoryPoint(810, 71.048230249288),
+    Boardcore::TrajectoryPoint(820, 69.4559691422748),
+    Boardcore::TrajectoryPoint(830, 67.8354856219309),
+    Boardcore::TrajectoryPoint(840, 66.1845729615503),
+    Boardcore::TrajectoryPoint(850, 64.5008099767691),
+    Boardcore::TrajectoryPoint(860, 62.7815353973901),
+    Boardcore::TrajectoryPoint(870, 61.0237672707407),
+    Boardcore::TrajectoryPoint(880, 59.2230550349347),
+    Boardcore::TrajectoryPoint(890, 57.3765852255011),
+    Boardcore::TrajectoryPoint(900, 55.4789538743064),
+    Boardcore::TrajectoryPoint(910, 53.5253686767019),
+    Boardcore::TrajectoryPoint(920, 51.5079968711945),
+    Boardcore::TrajectoryPoint(930, 49.4200993719904),
+    Boardcore::TrajectoryPoint(940, 47.252187731736),
+    Boardcore::TrajectoryPoint(950, 44.9918449898108),
+    Boardcore::TrajectoryPoint(960, 42.6250397738168),
+    Boardcore::TrajectoryPoint(970, 40.132289270307),
+    Boardcore::TrajectoryPoint(980, 37.4890974664655),
+    Boardcore::TrajectoryPoint(990, 34.6601134111677),
+    Boardcore::TrajectoryPoint(1000, 31.5959916494853),
+    Boardcore::TrajectoryPoint(1010, 28.22089221044),
+    Boardcore::TrajectoryPoint(1020, 24.40346033828),
+    Boardcore::TrajectoryPoint(1030, 19.8943620344018),
+    Boardcore::TrajectoryPoint(1040, 14.0420207204283),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::TrajectoryPoint t10_open[] = {
+    Boardcore::TrajectoryPoint(0, 168.255779968402),
+    Boardcore::TrajectoryPoint(10, 167.162192733818),
+    Boardcore::TrajectoryPoint(20, 166.069701861961),
+    Boardcore::TrajectoryPoint(30, 164.977719060261),
+    Boardcore::TrajectoryPoint(40, 163.886148579548),
+    Boardcore::TrajectoryPoint(50, 162.794908463837),
+    Boardcore::TrajectoryPoint(60, 161.703922748259),
+    Boardcore::TrajectoryPoint(70, 160.613111695649),
+    Boardcore::TrajectoryPoint(80, 159.522365835857),
+    Boardcore::TrajectoryPoint(90, 158.431604739666),
+    Boardcore::TrajectoryPoint(100, 157.340740491),
+    Boardcore::TrajectoryPoint(110, 156.249661729934),
+    Boardcore::TrajectoryPoint(120, 155.158304030038),
+    Boardcore::TrajectoryPoint(130, 154.066587958333),
+    Boardcore::TrajectoryPoint(140, 152.974362569214),
+    Boardcore::TrajectoryPoint(150, 151.881571371771),
+    Boardcore::TrajectoryPoint(160, 150.788158118517),
+    Boardcore::TrajectoryPoint(170, 149.693955671423),
+    Boardcore::TrajectoryPoint(180, 148.598882389628),
+    Boardcore::TrajectoryPoint(190, 147.502921857494),
+    Boardcore::TrajectoryPoint(200, 146.405849032977),
+    Boardcore::TrajectoryPoint(210, 145.307631163775),
+    Boardcore::TrajectoryPoint(220, 144.208258302506),
+    Boardcore::TrajectoryPoint(230, 143.107367746732),
+    Boardcore::TrajectoryPoint(240, 142.005126036057),
+    Boardcore::TrajectoryPoint(250, 140.90129129059),
+    Boardcore::TrajectoryPoint(260, 139.79574756635),
+    Boardcore::TrajectoryPoint(270, 138.688525043137),
+    Boardcore::TrajectoryPoint(280, 137.579235238258),
+    Boardcore::TrajectoryPoint(290, 136.468125989427),
+    Boardcore::TrajectoryPoint(300, 135.354663520507),
+    Boardcore::TrajectoryPoint(310, 134.239150218386),
+    Boardcore::TrajectoryPoint(320, 133.121067775264),
+    Boardcore::TrajectoryPoint(330, 132.000677517929),
+    Boardcore::TrajectoryPoint(340, 130.877442588637),
+    Boardcore::TrajectoryPoint(350, 129.751688190099),
+    Boardcore::TrajectoryPoint(360, 128.622739685996),
+    Boardcore::TrajectoryPoint(370, 127.490992807816),
+    Boardcore::TrajectoryPoint(380, 126.355865448186),
+    Boardcore::TrajectoryPoint(390, 125.21749960496),
+    Boardcore::TrajectoryPoint(400, 124.075678037563),
+    Boardcore::TrajectoryPoint(410, 122.9300502602),
+    Boardcore::TrajectoryPoint(420, 121.780805116912),
+    Boardcore::TrajectoryPoint(430, 120.627417255802),
+    Boardcore::TrajectoryPoint(440, 119.469862037486),
+    Boardcore::TrajectoryPoint(450, 118.308126175976),
+    Boardcore::TrajectoryPoint(460, 117.141713077783),
+    Boardcore::TrajectoryPoint(470, 115.97051988696),
+    Boardcore::TrajectoryPoint(480, 114.794519022964),
+    Boardcore::TrajectoryPoint(490, 113.613483505953),
+    Boardcore::TrajectoryPoint(500, 112.426801510304),
+    Boardcore::TrajectoryPoint(510, 111.234581540997),
+    Boardcore::TrajectoryPoint(520, 110.036601498587),
+    Boardcore::TrajectoryPoint(530, 108.832594901812),
+    Boardcore::TrajectoryPoint(540, 107.622283539544),
+    Boardcore::TrajectoryPoint(550, 106.405238735423),
+    Boardcore::TrajectoryPoint(560, 105.181212005869),
+    Boardcore::TrajectoryPoint(570, 103.95000659209),
+    Boardcore::TrajectoryPoint(580, 102.71129619815),
+    Boardcore::TrajectoryPoint(590, 101.46474009246),
+    Boardcore::TrajectoryPoint(600, 100.209982410363),
+    Boardcore::TrajectoryPoint(610, 98.9466514045362),
+    Boardcore::TrajectoryPoint(620, 97.6743586400159),
+    Boardcore::TrajectoryPoint(630, 96.3926981302938),
+    Boardcore::TrajectoryPoint(640, 95.101245410554),
+    Boardcore::TrajectoryPoint(650, 93.7995565436867),
+    Boardcore::TrajectoryPoint(660, 92.4870025170449),
+    Boardcore::TrajectoryPoint(670, 91.162986029288),
+    Boardcore::TrajectoryPoint(680, 89.8271787204284),
+    Boardcore::TrajectoryPoint(690, 88.4790399695698),
+    Boardcore::TrajectoryPoint(700, 87.1180030520955),
+    Boardcore::TrajectoryPoint(710, 85.7426986653437),
+    Boardcore::TrajectoryPoint(720, 84.3530906971291),
+    Boardcore::TrajectoryPoint(730, 82.9484785503123),
+    Boardcore::TrajectoryPoint(740, 81.5271331670396),
+    Boardcore::TrajectoryPoint(750, 80.0892846495736),
+    Boardcore::TrajectoryPoint(760, 78.6327899535974),
+    Boardcore::TrajectoryPoint(770, 77.1577353059767),
+    Boardcore::TrajectoryPoint(780, 75.6616824139358),
+    Boardcore::TrajectoryPoint(790, 74.1442903676924),
+    Boardcore::TrajectoryPoint(800, 72.6040789823789),
+    Boardcore::TrajectoryPoint(810, 71.0387604059336),
+    Boardcore::TrajectoryPoint(820, 69.4470811544908),
+    Boardcore::TrajectoryPoint(830, 67.8271634606104),
+    Boardcore::TrajectoryPoint(840, 66.1768005225803),
+    Boardcore::TrajectoryPoint(850, 64.4935710993356),
+    Boardcore::TrajectoryPoint(860, 62.7748138812089),
+    Boardcore::TrajectoryPoint(870, 61.0175649356654),
+    Boardcore::TrajectoryPoint(880, 59.2173365099757),
+    Boardcore::TrajectoryPoint(890, 57.3713342102218),
+    Boardcore::TrajectoryPoint(900, 55.4741695662676),
+    Boardcore::TrajectoryPoint(910, 53.5210180197682),
+    Boardcore::TrajectoryPoint(920, 51.5040771676524),
+    Boardcore::TrajectoryPoint(930, 49.4165922241171),
+    Boardcore::TrajectoryPoint(940, 47.249074561539),
+    Boardcore::TrajectoryPoint(950, 44.9891186596966),
+    Boardcore::TrajectoryPoint(960, 42.6226687526033),
+    Boardcore::TrajectoryPoint(970, 40.1302643034058),
+    Boardcore::TrajectoryPoint(980, 37.4873973327142),
+    Boardcore::TrajectoryPoint(990, 34.658724698043),
+    Boardcore::TrajectoryPoint(1000, 31.5948905139691),
+    Boardcore::TrajectoryPoint(1010, 28.2200605996832),
+    Boardcore::TrajectoryPoint(1020, 24.4028712375333),
+    Boardcore::TrajectoryPoint(1030, 19.8939960702102),
+    Boardcore::TrajectoryPoint(1040, 14.0418533936787),
+    Boardcore::TrajectoryPoint(1050, 0),
+};
+Boardcore::Trajectory t_closed[] = {
+    Boardcore::Trajectory{0.0, t0_closed, 106},
+    Boardcore::Trajectory{0.0, t1_closed, 106},
+    Boardcore::Trajectory{0.0, t2_closed, 106},
+    Boardcore::Trajectory{0.0, t3_closed, 106},
+    Boardcore::Trajectory{0.0, t4_closed, 106},
+    Boardcore::Trajectory{0.0, t5_closed, 106},
+    Boardcore::Trajectory{0.0, t6_closed, 106},
+    Boardcore::Trajectory{0.0, t7_closed, 106},
+    Boardcore::Trajectory{0.0, t8_closed, 106},
+    Boardcore::Trajectory{0.0, t9_closed, 106},
+    Boardcore::Trajectory{0.0, t10_closed, 106},
+};
+Boardcore::Trajectory t_open[] = {
+    Boardcore::Trajectory{0.0, t0_open, 106},
+    Boardcore::Trajectory{0.0, t1_open, 106},
+    Boardcore::Trajectory{0.0, t2_open, 106},
+    Boardcore::Trajectory{0.0, t3_open, 106},
+    Boardcore::Trajectory{0.0, t4_open, 106},
+    Boardcore::Trajectory{0.0, t5_open, 106},
+    Boardcore::Trajectory{0.0, t6_open, 106},
+    Boardcore::Trajectory{0.0, t7_open, 106},
+    Boardcore::Trajectory{0.0, t8_open, 106},
+    Boardcore::Trajectory{0.0, t9_open, 106},
+    Boardcore::Trajectory{0.0, t10_open, 106},
+};
+const Boardcore::TrajectorySet CLOSED_TRAJECTORY_SET(t_closed, 11);
+const Boardcore::TrajectorySet OPEN_TRAJECTORY_SET(t_open, 11);
+
+}  // namespace ABKTrajectories
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..475f8a22421812912a2c7d49bdcd1f4064ecd94c
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -0,0 +1,451 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Configs/ADAConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <utils/AeroUtils/AeroUtils.h>
+
+#include <functional>
+
+using namespace Boardcore;
+using namespace Common;
+using namespace std;
+
+namespace Main
+{
+ADAController::ADAController(TaskScheduler* sched)
+    : FSM(&ADAController::state_idle), ada(getADAKalmanConfig()),
+      scheduler(sched)
+{
+    // Subscribe the class to the topics
+    EventBroker::getInstance().subscribe(this, TOPIC_ADA);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+    // Set the default reference values
+    ada.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+}
+
+bool ADAController::start()
+{
+    // Add the task to the scheduler
+    size_t result = scheduler->addTask(bind(&ADAController::update, this),
+                                       ADAConfig::UPDATE_PERIOD,
+                                       TaskScheduler::Policy::RECOVER);
+
+    return ActiveObject::start() && result != 0;
+}
+
+void ADAController::update()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    PressureData barometerData =
+        modules.get<Sensors>()->getStaticPressure1LastSample();
+
+    // Get a snapshot of the situation. There is no need to synchronize because
+    // the getter are already thread safe with a PauseKernel
+    ADAControllerStatus status = getStatus();
+    ADAState state             = getADAState();
+
+    // The algorithm changes its actions depending on the FSM state
+    switch (status.state)
+    {
+        case ADAControllerState::ARMED:
+        case ADAControllerState::ACTIVE_DISARMED:
+        {
+            ada.update(barometerData.pressure);
+            break;
+        }
+        case ADAControllerState::SHADOW_MODE:
+        {
+            // During shadow-mode no event will be thrown
+            ada.update(barometerData.pressure);
+
+            // Check for apogees
+            if (state.verticalSpeed < ADAConfig::APOGEE_VERTICAL_SPEED_TARGET)
+            {
+                detectedApogees++;
+            }
+            else
+            {
+                // Apogees must be consecutive in order to be valid
+                detectedApogees = 0;
+            }
+
+            // Log the detected apogees during this phase
+            logStatus(status.state);
+
+            break;
+        }
+        case ADAControllerState::ACTIVE:
+        {
+            // During shadow-mode no event will be thrown
+            ada.update(barometerData.pressure);
+
+            // Check for apogees
+            if (state.verticalSpeed < ADAConfig::APOGEE_VERTICAL_SPEED_TARGET)
+            {
+                detectedApogees++;
+            }
+            else
+            {
+                // Apogees must be consecutive in order to be valid
+                detectedApogees = 0;
+            }
+
+            // Check if the number of apogees has reached the limit
+            if (detectedApogees > ADAConfig::APOGEE_N_SAMPLES)
+            {
+                EventBroker::getInstance().post(ADA_APOGEE_DETECTED, TOPIC_ADA);
+            }
+
+            // Log the detected apogees during this phase
+            logStatus(status.state);
+
+            break;
+        }
+        default:
+        {
+            break;
+        }
+    }
+    Logger::getInstance().log(getADAState());
+}
+
+void ADAController::calibrate()
+{
+    Stats pressure;
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    for (int i = 0; i < ADAConfig::CALIBRATION_SAMPLES_COUNT; i++)
+    {
+        PressureData data =
+            modules.get<Sensors>()->getStaticPressure1LastSample();
+        pressure.add(data.pressure);
+
+        miosix::Thread::sleep(ADAConfig::CALIBRATION_SLEEP_TIME);
+    }
+
+    // Set the pressure and temperature reference
+    ReferenceValues reference = ada.getReferenceValues();
+    reference.refPressure     = pressure.getStats().mean;
+
+    // clang-format off
+    reference.refAltitude     = Aeroutils::relAltitude(reference.refPressure, 
+                                                       reference.mslPressure, 
+                                                       reference.mslTemperature);
+    // clang-format on
+
+    // Update the algorithm reference values
+    {
+        miosix::PauseKernelLock l;
+        ada.setReferenceValues(reference);
+        ada.setKalmanConfig(getADAKalmanConfig());
+        ada.update(reference.refPressure);
+    }
+
+    EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
+}
+
+void ADAController::setReferenceAltitude(float altitude)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = ada.getReferenceValues();
+    reference.refAltitude     = altitude;
+    ada.setReferenceValues(reference);
+}
+
+void ADAController::setReferenceTemperature(float temperature)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = ada.getReferenceValues();
+
+    // The temperature is in degrees, converted in kelvin
+    reference.refTemperature = temperature + 273.15f;
+    ada.setReferenceValues(reference);
+}
+
+void ADAController::setReferenceValues(const ReferenceValues reference)
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    ada.setReferenceValues(reference);
+}
+
+ADAControllerStatus ADAController::getStatus()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return status;
+}
+
+ADAState ADAController::getADAState()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return ada.getState();
+}
+
+ReferenceValues ADAController::getReferenceValues()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return ada.getReferenceValues();
+}
+
+void ADAController::state_idle(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ADAControllerState::IDLE);
+        }
+        case ADA_CALIBRATE:
+        {
+            return transition(&ADAController::state_calibrating);
+        }
+    }
+}
+
+void ADAController::state_calibrating(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(ADAControllerState::CALIBRATING);
+
+            // Calibrate the ADA
+            calibrate();
+            break;
+        }
+        case ADA_READY:
+        {
+            return transition(&ADAController::state_ready);
+        }
+    }
+}
+
+void ADAController::state_ready(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ADAControllerState::READY);
+        }
+        case ADA_CALIBRATE:
+        {
+            return transition(&ADAController::state_calibrating);
+        }
+        case ADA_FORCE_START:
+        {
+            // Skip directly to the active/shadow mode mode
+            return transition(&ADAController::state_shadow_mode);
+        }
+        case FLIGHT_ARMED:
+        {
+            return transition(&ADAController::state_armed);
+        }
+    }
+}
+
+void ADAController::state_armed(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ADAControllerState::ARMED);
+        }
+        case FLIGHT_DISARMED:
+        {
+            return transition(&ADAController::state_ready);
+        }
+        case FLIGHT_LIFTOFF:
+        {
+            return transition(&ADAController::state_shadow_mode);
+        }
+    }
+}
+
+void ADAController::state_shadow_mode(const Event& event)
+{
+    static uint16_t shadowModeTimeoutEventId = 0;
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(ADAControllerState::SHADOW_MODE);
+
+            // Add a delayed event to exit the shadow mode
+            shadowModeTimeoutEventId = EventBroker::getInstance().postDelayed(
+                ADA_SHADOW_MODE_TIMEOUT, TOPIC_ADA,
+                ADAConfig::SHADOW_MODE_TIMEOUT);
+            break;
+        }
+        case EV_EXIT:
+        {
+            // Remove the shadow mode event. This works even though the event is
+            // expired (aka after shadow_mode_timeout) because the event broker
+            // assigns a progressive number for every delayed event. If and only
+            // if the number of registered delayed event is less than 2^16, then
+            // this technique is valid.
+            return EventBroker::getInstance().removeDelayed(
+                shadowModeTimeoutEventId);
+        }
+        case ADA_SHADOW_MODE_TIMEOUT:
+        {
+            return transition(&ADAController::state_active);
+        }
+        case ADA_FORCE_STOP:
+        {
+            return transition(&ADAController::state_ready);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&ADAController::state_end);
+        }
+    }
+}
+
+void ADAController::state_active(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Zero the number of so far detected apogees. It is thread safe due
+            // to std::atomic variable
+            detectedApogees = 0;
+            return logStatus(ADAControllerState::ACTIVE);
+        }
+        case ADA_FORCE_STOP:
+        {
+            return transition(&ADAController::state_ready);
+        }
+        case FLIGHT_APOGEE_DETECTED:
+        {
+            return transition(&ADAController::state_active_disarmed);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&ADAController::state_end);
+        }
+    }
+}
+
+void ADAController::state_active_disarmed(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ADAControllerState::ACTIVE_DISARMED);
+        }
+        case ADA_FORCE_STOP:
+        {
+            return transition(&ADAController::state_ready);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&ADAController::state_end);
+        }
+    }
+}
+
+void ADAController::state_end(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(ADAControllerState::END);
+        }
+    }
+}
+
+void ADAController::logStatus(ADAControllerState state)
+{
+    {
+        miosix::PauseKernelLock lock;
+        // Update the current FSM state
+        status.timestamp       = TimestampTimer::getTimestamp();
+        status.state           = state;
+        status.detectedApogees = detectedApogees;
+    }
+
+    // Log the status
+    Logger::getInstance().log(status);
+}
+
+ADA::KalmanFilter::KalmanConfig ADAController::getADAKalmanConfig()
+{
+    ADA::KalmanFilter::MatrixNN F_INIT;
+    ADA::KalmanFilter::MatrixPN H_INIT;
+    ADA::KalmanFilter::MatrixNN P_INIT;
+    ADA::KalmanFilter::MatrixNN Q_INIT;
+    ADA::KalmanFilter::MatrixPP R_INIT;
+    ADA::KalmanFilter::MatrixNM G_INIT;
+
+    // clang-format off
+    F_INIT  = ADA::KalmanFilter::MatrixNN({
+        {1.0, ADAConfig::SAMPLING_PERIOD,    0.5f * ADAConfig::SAMPLING_PERIOD * ADAConfig::SAMPLING_PERIOD},
+        {0.0, 1.0,                           ADAConfig::SAMPLING_PERIOD},
+        {0.0, 0.0,                           1.0}});
+    // clang-format on
+
+    H_INIT = {1.0, 0.0, 0.0};
+
+    P_INIT = ADA::KalmanFilter::MatrixNN(
+        {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}});
+
+    Q_INIT << ADA::KalmanFilter::MatrixNN(
+        {{30.0, 0.0, 0.0}, {0.0, 10.0, 0.0}, {0.0, 0.0, 2.5f}});
+
+    R_INIT[0] = 4000.0f;
+
+    G_INIT = ADA::KalmanFilter::MatrixNM::Zero();
+
+    return {F_INIT,
+            H_INIT,
+            Q_INIT,
+            R_INIT,
+            P_INIT,
+            G_INIT,
+            ADA::KalmanFilter::CVectorN(ada.getReferenceValues().refPressure, 0,
+                                        0)};
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
new file mode 100644
index 0000000000000000000000000000000000000000..66474c5c953e4a8856a87a21e195410868e9f9d6
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -0,0 +1,97 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/StateMachines/ADAController/ADAControllerData.h>
+#include <algorithms/ADA/ADA.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class ADAController : public Boardcore::Module,
+                      public Boardcore::FSM<ADAController>
+{
+public:
+    ADAController(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the FSM thread and adds an update function into the
+     * scheduler
+     */
+    bool start() override;
+
+    /**
+     * @brief Update function called periodically by the scheduler. It checks
+     * the current FSM state and checks for apogees.
+     */
+    void update();
+
+    void calibrate();
+
+    // ADA setters
+    void setReferenceAltitude(float altitude);
+    void setReferenceTemperature(float temperature);
+    void setReferenceValues(const Boardcore::ReferenceValues reference);
+
+    // ADA getters
+    ADAControllerStatus getStatus();
+    Boardcore::ADAState getADAState();
+    Boardcore::ReferenceValues getReferenceValues();
+
+    // FSM states
+    void state_idle(const Boardcore::Event& event);
+    void state_calibrating(const Boardcore::Event& event);
+    void state_ready(const Boardcore::Event& event);
+    void state_armed(const Boardcore::Event& event);
+    void state_shadow_mode(const Boardcore::Event& event);
+    void state_active(const Boardcore::Event& event);
+    void state_active_disarmed(const Boardcore::Event& event);
+    void state_end(const Boardcore::Event& event);
+
+private:
+    /**
+     * @brief Logs the ADA status updating the FSM state
+     * @param state The current FSM state
+     */
+    void logStatus(ADAControllerState state);
+
+    // Returns the ADA kalman matrices that configure the filter
+    Boardcore::ADA::KalmanFilter::KalmanConfig getADAKalmanConfig();
+
+    // Controller state machine status
+    ADAControllerStatus status;
+    Boardcore::ADA ada;
+
+    // Scheduler to be used for update function
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Thread safe counter for apogees
+    std::atomic<uint16_t> detectedApogees{0};
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ADA");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b19c94467ea8d0436b6441ee064ccc33361578a
--- /dev/null
+++ b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
@@ -0,0 +1,61 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+
+enum class ADAControllerState : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    CALIBRATING,
+    READY,
+    ARMED,
+    SHADOW_MODE,
+    ACTIVE,
+    ACTIVE_DISARMED,
+    END
+};
+
+struct ADAControllerStatus
+{
+    uint64_t timestamp       = 0;
+    uint16_t detectedApogees = 0;
+    ADAControllerState state = ADAControllerState::UNINIT;
+
+    static std::string header() { return "timestamp,detectedApogees,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)detectedApogees << "," << (int)state
+           << "\n";
+    }
+};
+
+}  // namespace Main
diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.cpp b/src/boards/Main/StateMachines/Deployment/Deployment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..235a1819dcf46c8e0e40f27c2a8edaecd87a4d0b
--- /dev/null
+++ b/src/boards/Main/StateMachines/Deployment/Deployment.cpp
@@ -0,0 +1,108 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/Configs/DeploymentConfig.h>
+#include <Main/StateMachines/Deployment/Deployment.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace Common;
+
+namespace Main
+{
+Deployment::Deployment() : FSM(&Deployment::state_idle)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_DPL);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+DeploymentStatus Deployment::getStatus()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return status;
+}
+
+void Deployment::state_idle(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            modules.get<Actuators>()->cutterOff();
+            return logStatus(DeploymentState::IDLE);
+        }
+        case FLIGHT_DPL_ALT_DETECTED:
+        {
+            return transition(&Deployment::state_cutting);
+        }
+    }
+}
+
+void Deployment::state_cutting(const Event& event)
+{
+    static uint16_t ncCuttingTimeoutEventId = 0;
+
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(DeploymentState::CUTTING);
+            modules.get<Actuators>()->cutterOn();
+
+            ncCuttingTimeoutEventId = EventBroker::getInstance().postDelayed(
+                DPL_CUT_TIMEOUT, TOPIC_DPL, DeploymentConfig::CUT_DURATION);
+
+            break;
+        }
+        case EV_EXIT:
+        {
+            return EventBroker::getInstance().removeDelayed(
+                ncCuttingTimeoutEventId);
+        }
+        case DPL_CUT_TIMEOUT:
+        {
+            return transition(&Deployment::state_idle);
+        }
+    }
+}
+
+void Deployment::logStatus(DeploymentState state)
+{
+    {
+        miosix::PauseKernelLock lock;
+        // Update the current FSM state
+        status.timestamp = TimestampTimer::getTimestamp();
+        status.state     = state;
+    }
+
+    // Log the status
+    Logger::getInstance().log(status);
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.h b/src/boards/Main/StateMachines/Deployment/Deployment.h
new file mode 100644
index 0000000000000000000000000000000000000000..b928681173a6b8ebf9dd94d1c128b200457a8c3c
--- /dev/null
+++ b/src/boards/Main/StateMachines/Deployment/Deployment.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/StateMachines/Deployment/DeploymentData.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class Deployment : public Boardcore::Module, public Boardcore::FSM<Deployment>
+{
+public:
+    Deployment();
+
+    DeploymentStatus getStatus();
+
+    // FSM states
+    void state_idle(const Boardcore::Event& event);
+    void state_cutting(const Boardcore::Event& event);
+
+private:
+    /**
+     * @brief Logs the Deployment status updating the FSM state
+     * @param state The current FSM state
+     */
+    void logStatus(DeploymentState state);
+
+    // State machine status
+    DeploymentStatus status;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Deployment");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/Deployment/DeploymentData.h b/src/boards/Main/StateMachines/Deployment/DeploymentData.h
new file mode 100644
index 0000000000000000000000000000000000000000..edf440bdff0855a47f4c7690bf774d8de0f7c282
--- /dev/null
+++ b/src/boards/Main/StateMachines/Deployment/DeploymentData.h
@@ -0,0 +1,51 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+enum class DeploymentState : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    CUTTING,
+};
+
+struct DeploymentStatus
+{
+    uint64_t timestamp    = 0;
+    DeploymentState state = DeploymentState::UNINIT;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52cbd188dcb1ae2e773d67c93d28f1ab47ead009
--- /dev/null
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -0,0 +1,742 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Angelo Prete, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/CanHandler/CanHandler.h>
+#include <Main/Configs/FlightModeManagerConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+
+#include "FlightModeManagerData.h"
+
+using namespace Boardcore;
+using namespace Common;
+using namespace miosix;
+using namespace Main::FMMConfig;
+
+namespace Main
+{
+FlightModeManager::FlightModeManager()
+    : HSM(&FlightModeManager::state_on_ground)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+    EventBroker::getInstance().subscribe(this, TOPIC_FMM);
+    EventBroker::getInstance().subscribe(this, TOPIC_TMTC);
+    EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+    EventBroker::getInstance().subscribe(this, TOPIC_CAN);
+    EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+    EventBroker::getInstance().subscribe(this, TOPIC_ADA);
+    EventBroker::getInstance().subscribe(this, TOPIC_ALT);
+    EventBroker::getInstance().subscribe(this, TOPIC_MEA);
+}
+
+FlightModeManagerStatus FlightModeManager::getStatus()
+{
+    PauseKernelLock lock;
+    return status;
+}
+
+State FlightModeManager::state_on_ground(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::ON_GROUND);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_top);
+        }
+        case EV_INIT:
+        {
+            return transition(&FlightModeManager::state_init);
+        }
+        case TMTC_RESET_BOARD:
+        {
+            reboot();
+            return HANDLED;
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_init(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::INIT);
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case FMM_INIT_ERROR:
+        {
+            return transition(&FlightModeManager::state_init_error);
+        }
+        case FMM_INIT_OK:
+        {
+            return transition(&FlightModeManager::state_init_done);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_init_error(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::INIT_ERROR);
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case CAN_FORCE_INIT:
+        case TMTC_FORCE_INIT:
+        {
+            if (event != CAN_FORCE_INIT)
+            {
+                modules.get<CanHandler>()->sendEvent(
+                    CanConfig::EventId::FORCE_INIT);
+            }
+            return transition(&FlightModeManager::state_init_done);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_init_done(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::INIT_DONE);
+            EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM);
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case FMM_CALIBRATE:
+        {
+            return transition(&FlightModeManager::state_calibrate_sensors);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_calibrate_sensors(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::CALIBRATE_SENSORS);
+
+            ModuleManager::getInstance().get<Sensors>()->calibrate();
+            EventBroker::getInstance().post(FMM_SENSORS_CAL_DONE, TOPIC_FMM);
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case FMM_SENSORS_CAL_DONE:
+        {
+            return transition(&FlightModeManager::state_calibrate_algorithms);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_calibrate_algorithms(const Event& event)
+{
+    static bool nasReady = false;
+    static bool adaReady = false;
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::CALIBRATE_ALGORITHMS);
+
+            // Reset the readiness variables (not doing this could lead to
+            // mis-calibration in future attempts)
+            nasReady = false;
+            adaReady = false;
+
+            // Post the calibration events
+            EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS);
+            EventBroker::getInstance().post(ADA_CALIBRATE, TOPIC_ADA);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case NAS_READY:
+        {
+            nasReady = true;
+
+            if (adaReady)
+            {
+                return transition(&FlightModeManager::state_disarmed);
+            }
+            else
+            {
+                return HANDLED;
+            }
+        }
+        case ADA_READY:
+        {
+            adaReady = true;
+
+            if (nasReady)
+            {
+                return transition(&FlightModeManager::state_disarmed);
+            }
+            else
+            {
+                return HANDLED;
+            }
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_disarmed(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::DISARMED);
+
+            // Stop eventual logging
+            Logger::getInstance().stop();
+            modules.get<Actuators>()->camOff();
+            modules.get<Actuators>()->setBuzzerOff();
+            EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case CAN_CALIBRATE:
+        case TMTC_CALIBRATE:
+        {
+            if (event != CAN_CALIBRATE)
+            {
+                modules.get<CanHandler>()->sendEvent(
+                    CanConfig::EventId::CALIBRATE);
+            }
+            return transition(&FlightModeManager::state_calibrate_sensors);
+        }
+        case CAN_ENTER_TEST_MODE:
+        case TMTC_ENTER_TEST_MODE:
+        {
+            if (event != CAN_ENTER_TEST_MODE)
+            {
+                modules.get<CanHandler>()->sendEvent(
+                    CanConfig::EventId::ENTER_TEST_MODE);
+            }
+            return transition(&FlightModeManager::state_test_mode);
+        }
+        case CAN_ARM:
+        case TMTC_ARM:
+        {
+            if (event != CAN_ARM)
+            {
+                modules.get<CanHandler>()->sendEvent(CanConfig::EventId::ARM);
+            }
+            return transition(&FlightModeManager::state_armed);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_test_mode(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::TEST_MODE);
+
+            Logger::getInstance().start();
+            EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            Logger::getInstance().stop();
+            EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS);
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_on_ground);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case TMTC_START_RECORDING:
+        {
+            modules.get<Actuators>()->camOn();
+            return HANDLED;
+        }
+        case TMTC_STOP_RECORDING:
+        {
+            modules.get<Actuators>()->camOff();
+            return HANDLED;
+        }
+        case CAN_EXIT_TEST_MODE:
+        case TMTC_EXIT_TEST_MODE:
+        {
+            if (event != CAN_EXIT_TEST_MODE)
+            {
+                modules.get<CanHandler>()->sendEvent(
+                    CanConfig::EventId::EXIT_TEST_MODE);
+            }
+            return transition(&FlightModeManager::state_disarmed);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_armed(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::ARMED);
+
+            Logger::getInstance().start();
+            modules.get<Actuators>()->camOn();
+            modules.get<Actuators>()->setBuzzerArm();
+            EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_top);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case CAN_DISARM:
+        case TMTC_DISARM:
+        {
+            if (event != CAN_DISARM)
+            {
+                modules.get<CanHandler>()->sendEvent(
+                    CanConfig::EventId::DISARM);
+            }
+            return transition(&FlightModeManager::state_disarmed);
+        }
+        case FLIGHT_LAUNCH_PIN_DETACHED:
+        case TMTC_FORCE_LAUNCH:
+        {
+            modules.get<CanHandler>()->sendEvent(CanConfig::EventId::LIFTOFF);
+            return transition(&FlightModeManager::state_flying);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_flying(const Event& event)
+{
+    static uint16_t missionTimeoutEventId = 0;
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::FLYING);
+
+            EventBroker::getInstance().post(FLIGHT_LIFTOFF, TOPIC_FLIGHT);
+            missionTimeoutEventId = EventBroker::getInstance().postDelayed(
+                FLIGHT_MISSION_TIMEOUT, TOPIC_FLIGHT, MISSION_TIMEOUT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            EventBroker::getInstance().removeDelayed(missionTimeoutEventId);
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_top);
+        }
+        case EV_INIT:
+        {
+            return transition(&FlightModeManager::state_powered_ascent);
+        }
+        case FLIGHT_MISSION_TIMEOUT:
+        {
+            EventBroker::getInstance().post(FLIGHT_LANDING_TIMEOUT,
+                                            TOPIC_FLIGHT);
+            return transition(&FlightModeManager::state_landed);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_powered_ascent(const Event& event)
+{
+    static uint16_t engineShutdownTimeoutEventId = 0;
+    ModuleManager& modules                       = ModuleManager::getInstance();
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::POWERED_ASCENT);
+
+            // After a maximum time, the motor is considered shut down
+            engineShutdownTimeoutEventId =
+                EventBroker::getInstance().postDelayed(
+                    MOTOR_CLOSE_FEED_VALVE, TOPIC_FMM, ENGINE_SHUTDOWN_TIMEOUT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            // Shutdown via can
+            modules.get<CanHandler>()->sendCanCommand(ServosList::MAIN_VALVE, 0,
+                                                      ENGINE_SHUTDOWN_TIMEOUT);
+
+            EventBroker::getInstance().removeDelayed(
+                engineShutdownTimeoutEventId);
+
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_flying);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case MEA_SHUTDOWN_DETECTED:
+        case MOTOR_CLOSE_FEED_VALVE:
+        {
+            return transition(&FlightModeManager::state_unpowered_ascent);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_unpowered_ascent(const Event& event)
+{
+    static uint16_t apogeeTimeoutEventId = 0;
+    ModuleManager& modules               = ModuleManager::getInstance();
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::UNPOWERED_ASCENT);
+            EventBroker::getInstance().post(FLIGHT_MOTOR_SHUTDOWN,
+                                            TOPIC_FLIGHT);
+            apogeeTimeoutEventId = EventBroker::getInstance().postDelayed(
+                TMTC_FORCE_APOGEE, TOPIC_TMTC, APOGEE_EVENT_TIMEOUT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            modules.get<Actuators>()->setServoPosition(
+                ServosList::EXPULSION_SERVO, 1);
+
+            EventBroker::getInstance().removeDelayed(apogeeTimeoutEventId);
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_flying);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case TMTC_FORCE_APOGEE:
+        case ADA_APOGEE_DETECTED:
+        {
+            modules.get<CanHandler>()->sendEvent(
+                CanConfig::EventId::APOGEE_DETECTED);
+            return transition(&FlightModeManager::state_drogue_descent);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_drogue_descent(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::DROGUE_DESCENT);
+            EventBroker::getInstance().post(FLIGHT_APOGEE_DETECTED,
+                                            TOPIC_FLIGHT);
+            EventBroker::getInstance().post(FLIGHT_DROGUE_DESCENT,
+                                            TOPIC_FLIGHT);
+
+            // Open the venting valve
+            modules.get<CanHandler>()->sendCanCommand(ServosList::VENTING_VALVE,
+                                                      1, MISSION_TIMEOUT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_flying);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case TMTC_FORCE_DEPLOYMENT:
+        case ALTITUDE_TRIGGER_ALTITUDE_REACHED:
+        {
+            return transition(&FlightModeManager::state_terminal_descent);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_terminal_descent(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::TERMINAL_DESCENT);
+            EventBroker::getInstance().post(FLIGHT_DPL_ALT_DETECTED,
+                                            TOPIC_FLIGHT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_flying);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case TMTC_FORCE_LANDING:
+        {
+            return transition(&FlightModeManager::state_landed);
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+State FlightModeManager::state_landed(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(FlightModeManagerState::LANDED);
+            modules.get<Actuators>()->setBuzzerLand();
+            EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED,
+                                            TOPIC_FLIGHT);
+
+            return HANDLED;
+        }
+        case EV_EXIT:
+        {
+            return HANDLED;
+        }
+        case EV_EMPTY:
+        {
+            return tranSuper(&FlightModeManager::state_top);
+        }
+        case EV_INIT:
+        {
+            return HANDLED;
+        }
+        case TMTC_RESET_BOARD:
+        {
+            reboot();
+            return HANDLED;
+        }
+        default:
+        {
+            return UNHANDLED;
+        }
+    }
+}
+
+void FlightModeManager::logStatus(FlightModeManagerState state)
+{
+    {
+        PauseKernelLock lock;
+        status.timestamp = TimestampTimer::getTimestamp();
+        status.state     = state;
+    }
+
+    Logger::getInstance().log(status);
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ab34b0bed7ad9a55c81315a5e460e0d44e03cd5
--- /dev/null
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -0,0 +1,101 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Angelo Prete, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <events/EventBroker.h>
+#include <events/HSM.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "FlightModeManagerData.h"
+
+namespace Main
+{
+
+class FlightModeManager : public Boardcore::Module,
+                          public Boardcore::HSM<FlightModeManager>
+{
+public:
+    FlightModeManager();
+
+    FlightModeManagerStatus getStatus();
+
+    // Super state for when the rocket is on ground
+    Boardcore::State state_on_ground(const Boardcore::Event& event);
+
+    // Initialization state
+    Boardcore::State state_init(const Boardcore::Event& event);
+
+    // State in which the init has failed
+    Boardcore::State state_init_error(const Boardcore::Event& event);
+
+    // State in which the init is done and a calibration event is thrown
+    Boardcore::State state_init_done(const Boardcore::Event& event);
+
+    // Calibration of all the sensors (offsets)
+    Boardcore::State state_calibrate_sensors(const Boardcore::Event& event);
+
+    // Calibration of all the algorithms (triad for NAS etc..)
+    Boardcore::State state_calibrate_algorithms(const Boardcore::Event& event);
+
+    // State in which the electronics is ready to be armed
+    Boardcore::State state_disarmed(const Boardcore::Event& event);
+
+    // State in which some actions (like main deployment, expulsion event etc..)
+    // are accepted
+    Boardcore::State state_test_mode(const Boardcore::Event& event);
+
+    // State in which the algorithms start to run (NAS) and the electronics is
+    // ready to fly
+    Boardcore::State state_armed(const Boardcore::Event& event);
+
+    // Super state in which the liftoff has been detected
+    Boardcore::State state_flying(const Boardcore::Event& event);
+
+    // State in which the the rocket is ascending with the motor on. The
+    // automatic shutdown algorithm runs during this phase
+    Boardcore::State state_powered_ascent(const Boardcore::Event& event);
+
+    // State in which the motor has been shut down. The airbrakes algorithm is
+    // running during this phase
+    Boardcore::State state_unpowered_ascent(const Boardcore::Event& event);
+
+    // State in which the apogee has been detected (triggered by the ADA system)
+    Boardcore::State state_drogue_descent(const Boardcore::Event& event);
+
+    // State in which the parachute has been opened by an altitude trigger
+    Boardcore::State state_terminal_descent(const Boardcore::Event& event);
+
+    // State in which n minutes of time have passed after the liftoff event
+    Boardcore::State state_landed(const Boardcore::Event& event);
+
+private:
+    void logStatus(FlightModeManagerState state);
+
+    FlightModeManagerStatus status;
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("FlightModeManager");
+};
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9ede12b227b1fd0e443f4882e20005213748b26
--- /dev/null
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h
@@ -0,0 +1,67 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Angelo Prete, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+
+enum class FlightModeManagerState : uint8_t
+{
+    ON_GROUND = 0,
+    INIT,
+    INIT_ERROR,
+    INIT_DONE,
+    CALIBRATE_SENSORS,
+    CALIBRATE_ALGORITHMS,
+    DISARMED,
+    TEST_MODE,
+    ARMED,
+    IGNITION,
+    FLYING,
+    POWERED_ASCENT,
+    UNPOWERED_ASCENT,
+    DROGUE_DESCENT,
+    TERMINAL_DESCENT,
+    LANDED,
+    INVALID,
+};
+
+struct FlightModeManagerStatus
+{
+    uint64_t timestamp           = 0;
+    FlightModeManagerState state = FlightModeManagerState::INVALID;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a59e2de05b50fb66a1dfd0fab26a42d12c485acd
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
@@ -0,0 +1,403 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/Configs/MEAConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+#include <functional>
+
+using namespace Boardcore;
+using namespace Common;
+using namespace std;
+
+namespace Main
+{
+MEAController::MEAController(TaskScheduler* sched)
+    : FSM(&MEAController::state_idle), mea(getMEAKalmanConfig()),
+      scheduler(sched)
+{
+    // Subscribe the class of the topics
+    EventBroker::getInstance().subscribe(this, TOPIC_MEA);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+bool MEAController::start()
+{
+    size_t result = scheduler->addTask(bind(&MEAController::update, this),
+                                       MEAConfig::UPDATE_PERIOD,
+                                       TaskScheduler::Policy::RECOVER);
+    return ActiveObject::start() && result != 0;
+}
+
+void MEAController::update()
+{
+    ModuleManager& modules  = ModuleManager::getInstance();
+    PressureData ccPressure = modules.get<Sensors>()->getCCPressureLastSample();
+
+    // No need for pause kernel due to its presence inside the getter
+    MEAControllerStatus status = getStatus();
+    NASState nasState          = modules.get<NASController>()->getNasState();
+
+    // Get mach number and estimated CD
+    float mach = computeMach(nasState);
+    float CD   = computeCD(mach);
+    float rho  = computeRho(nasState);
+
+    // Get MAIN valve state from CAN bus
+    float valvePosition =
+        modules.get<Actuators>()->getServoPosition(ServosList::MAIN_VALVE) > 0.3
+            ? 1
+            : 0;
+
+    // If the state is active (or armed) and the pressure is greater than the
+    // threshold update the kalman
+    switch (status.state)
+    {
+        case MEAControllerState::ARMED:
+        {
+            if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD &&
+                ccPressure.pressureTimestamp > lastUpdateTimestamp)
+            {
+                mea.update(valvePosition, ccPressure.pressure);
+                lastUpdateTimestamp = TimestampTimer::getTimestamp();
+            }
+            break;
+        }
+        case MEAControllerState::SHADOW_MODE:
+        {
+            if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD &&
+                ccPressure.pressureTimestamp > lastUpdateTimestamp)
+            {
+                mea.update(valvePosition, ccPressure.pressure);
+                lastUpdateTimestamp = TimestampTimer::getTimestamp();
+            }
+
+            // Compute the estimated altitude
+            estimatedAltitude =
+                computeAltitude(nasState, mea.getState().x2, CD, 0.15f, rho);
+            estimatedMass = mea.getState().x2;
+
+            if (estimatedAltitude >= MEAConfig::SHUTDOWN_THRESHOLD_ALTITUDE)
+            {
+                detectedShutdowns++;
+            }
+            else
+            {
+                // Shutdowns must be consecutive in order to be valid
+                detectedShutdowns = 0;
+            }
+
+            logStatus(status.state);
+            break;
+        }
+        case MEAControllerState::ACTIVE:
+        {
+            if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD &&
+                ccPressure.pressureTimestamp > lastUpdateTimestamp)
+            {
+                mea.update(valvePosition, ccPressure.pressure);
+                lastUpdateTimestamp = TimestampTimer::getTimestamp();
+            }
+
+            // Compute the estimated altitude
+            estimatedAltitude =
+                computeAltitude(nasState, mea.getState().x2, CD, 0.15f, rho);
+            estimatedMass = mea.getState().x2;
+
+            if (estimatedAltitude >= MEAConfig::SHUTDOWN_THRESHOLD_ALTITUDE)
+            {
+                detectedShutdowns++;
+            }
+            else
+            {
+                // Shutdowns must be consecutive in order to be valid
+                detectedShutdowns = 0;
+            }
+
+            if (detectedShutdowns > MEAConfig::SHUTDOWN_N_SAMPLES)
+            {
+                EventBroker::getInstance().post(MEA_SHUTDOWN_DETECTED,
+                                                TOPIC_MEA);
+            }
+
+            logStatus(status.state);
+            break;
+        }
+        case MEAControllerState::ACTIVE_DISARMED:
+        {
+            if (ccPressure.pressure >= MEAConfig::CC_PRESSURE_THRESHOLD &&
+                ccPressure.pressureTimestamp > lastUpdateTimestamp)
+            {
+                mea.update(valvePosition, ccPressure.pressure);
+                lastUpdateTimestamp = TimestampTimer::getTimestamp();
+            }
+
+            estimatedMass = mea.getState().x2;
+
+            logStatus(status.state);
+            break;
+        }
+        default:
+        {
+            break;
+        }
+    }
+
+    Logger::getInstance().log(getMEAState());
+}
+
+MEAControllerStatus MEAController::getStatus()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return status;
+}
+
+MEAState MEAController::getMEAState()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return mea.getState();
+}
+
+void MEAController::state_idle(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(MEAControllerState::IDLE);
+
+            return transition(&MEAController::state_ready);
+        }
+    }
+}
+
+void MEAController::state_ready(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(MEAControllerState::READY);
+        }
+        case FLIGHT_ARMED:
+        {
+            return transition(&MEAController::state_armed);
+        }
+    }
+}
+
+void MEAController::state_armed(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(MEAControllerState::ARMED);
+        }
+        case FLIGHT_DISARMED:
+        {
+            return transition(&MEAController::state_ready);
+        }
+        case FLIGHT_LIFTOFF:
+        {
+            return transition(&MEAController::state_shadow_mode);
+        }
+    }
+}
+
+void MEAController::state_shadow_mode(const Event& event)
+{
+    static uint16_t shadowModeTimeoutEventId = 0;
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(MEAControllerState::SHADOW_MODE);
+
+            // Add a delayed event to exit the shadow mode
+            shadowModeTimeoutEventId = EventBroker::getInstance().postDelayed(
+                MOTOR_SHADOW_MODE_TIMEOUT, TOPIC_MEA,
+                MEAConfig::SHADOW_MODE_TIMEOUT);
+            break;
+        }
+        case EV_EXIT:
+        {
+            // Remove the shadow mode event. This works even though the event is
+            // expired (aka after shadow_mode_timeout) because the event broker
+            // assigns a progressive number for every delayed event. If and only
+            // if the number of registered delayed event is less than 2^16, then
+            // this technique is valid.
+            return EventBroker::getInstance().removeDelayed(
+                shadowModeTimeoutEventId);
+        }
+        case MOTOR_SHADOW_MODE_TIMEOUT:
+        {
+            return transition(&MEAController::state_active);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&MEAController::state_end);
+        }
+    }
+}
+
+void MEAController::state_active(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Zero the number of so far detected shutdowns. It is thread safe
+            // due to std::atomic variable
+            detectedShutdowns = 0;
+            return logStatus(MEAControllerState::ACTIVE);
+        }
+        case FLIGHT_MOTOR_SHUTDOWN:
+        {
+            return transition(&MEAController::state_active_disarmed);
+        }
+        case FLIGHT_APOGEE_DETECTED:
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&MEAController::state_end);
+        }
+    }
+}
+
+void MEAController::state_active_disarmed(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(MEAControllerState::ACTIVE_DISARMED);
+        }
+        case FLIGHT_APOGEE_DETECTED:
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&MEAController::state_end);
+        }
+    }
+}
+
+void MEAController::state_end(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(MEAControllerState::END);
+        }
+    }
+}
+
+void MEAController::logStatus(MEAControllerState state)
+{
+    {
+        miosix::PauseKernelLock lock;
+        status.timestamp         = TimestampTimer::getTimestamp();
+        status.state             = state;
+        status.detectedShutdowns = detectedShutdowns;
+        status.estimatedApogee   = estimatedAltitude;
+        status.estimatedMass     = estimatedMass;
+    }
+
+    Logger::getInstance().log(status);
+}
+
+float MEAController::computeMach(NASState state)
+{
+    float T = Constants::MSL_TEMPERATURE + Constants::a * (-state.d);
+    float c = sqrt(1.4f * Constants::R * T);
+    return -state.vd / c;
+}
+
+float MEAController::computeCD(float mach)
+{
+    return MEAConfig::n000 + MEAConfig::n100 * mach +
+           MEAConfig::n200 * powf(mach, 2) + MEAConfig::n300 * powf(mach, 3) +
+           MEAConfig::n400 * powf(mach, 4) + MEAConfig::n500 * powf(mach, 5) +
+           MEAConfig::n600 * powf(mach, 6);
+}
+
+float MEAController::computeAltitude(NASState state, float mass, float CD,
+                                     float D, float rho)
+{
+    // Compute rocket surface
+    float S = Constants::PI * (D / 2) * (D / 2);
+
+    return -state.d +
+           1 / (2 * (0.5 * rho * CD * S / mass)) *
+               log1p(((-state.vd) * (-state.vd) * (0.5 * rho * CD * S) / mass) /
+                     Constants::g);
+}
+
+float MEAController::computeRho(NASState state)
+{
+    return Constants::RHO_0 *
+           exp(state.d / 11000.f);  // 11000 is the troposphere height
+}
+
+MEA::KalmanFilter::KalmanConfig MEAController::getMEAKalmanConfig()
+{
+    MEA::KalmanFilter::MatrixNN F_INIT;
+    MEA::KalmanFilter::MatrixPN H_INIT;
+    MEA::KalmanFilter::MatrixNN P_INIT;
+    MEA::KalmanFilter::MatrixNN Q_INIT;
+    MEA::KalmanFilter::MatrixPP R_INIT;
+    MEA::KalmanFilter::MatrixNM G_INIT;
+
+    // clang-format off
+    F_INIT = MEA::KalmanFilter::MatrixNN({
+            {1.435871191228868, -0.469001276508780,  0.f}, 
+            {1.f,                0.f,                0.f},
+            {-0.002045309260755, 0.001867496708935,  1.f}});
+    
+    H_INIT = {1.780138883879285,-1.625379384370081,0.f};
+
+    P_INIT    = MEA::KalmanFilter::MatrixNN::Zero();
+    Q_INIT    = MEAConfig::MODEL_NOISE_VARIANCE * MEA::KalmanFilter::CVectorN({1, 1, 1}).asDiagonal();
+    R_INIT[0] = MEAConfig::SENSOR_NOISE_VARIANCE;
+    G_INIT    = MEA::KalmanFilter::MatrixNM{{4}, {0}, {0}};
+    // clang-format on
+
+    return {F_INIT,
+            H_INIT,
+            Q_INIT,
+            R_INIT,
+            P_INIT,
+            G_INIT,
+            MEA::KalmanFilter::CVectorN{
+                0, 0, MEAConfig::DEFAULT_INITIAL_ROCKET_MASS}};
+}
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main/StateMachines/MEAController/MEAController.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1e1ae6310efa3b3c1824713ab862fb2cc01fa8d
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.h
@@ -0,0 +1,121 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <Main/StateMachines/MEAController/MEAControllerData.h>
+#include <algorithms/MEA/MEA.h>
+#include <algorithms/NAS/NASState.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/Constants.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class MEAController : public Boardcore::Module,
+                      public Boardcore::FSM<MEAController>
+{
+public:
+    MEAController(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the FSM thread and adds an update function into the
+     * scheduler.
+     */
+    bool start() override;
+
+    /**
+     * @brief Update function called periodically by the scheduler. It checks
+     * the current FSM state and checks for engine shutdown.
+     */
+    void update();
+
+    // MEA getters
+    MEAControllerStatus getStatus();
+    Boardcore::MEAState getMEAState();
+
+    // FSM states
+    void state_idle(const Boardcore::Event& event);
+    void state_ready(const Boardcore::Event& event);
+    void state_armed(const Boardcore::Event& event);
+    void state_shadow_mode(const Boardcore::Event& event);
+    void state_active(const Boardcore::Event& event);
+    void state_active_disarmed(const Boardcore::Event& event);
+    void state_end(const Boardcore::Event& event);
+
+private:
+    /**
+     * @brief Logs the MEA status updating the FSM state
+     * @param state The current FSM state
+     */
+    void logStatus(MEAControllerState state);
+
+    /**
+     * @brief Computes the mach speed using the NASState
+     */
+    float computeMach(Boardcore::NASState state);
+
+    /**
+     * @brief Computes the rocket CD based on current speed and the coefficients
+     */
+    float computeCD(float mach);
+
+    /**
+     * @brief Estimates the altitude based on the estimated mass and current CD
+     *
+     * @param state NASState for estimated velocity and height
+     * @param mass Estimated mass
+     * @param CD estimated CD from velocity and height
+     * @param D diameter of the rocket
+     * @param rho air density at altitude h
+     */
+    float computeAltitude(Boardcore::NASState state, float mass, float CD,
+                          float D, float rho);
+
+    /**
+     * @brief Computes the RHO air density for standard isa air model
+     */
+    float computeRho(Boardcore::NASState state);
+
+    // Returns the MEA kalman matrices to configure the filter
+    Boardcore::MEA::KalmanFilter::KalmanConfig getMEAKalmanConfig();
+
+    // Controller state machine status
+    MEAControllerStatus status;
+    Boardcore::MEA mea;
+
+    // Scheduler to be used for update function
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Thread safe counter for engine shutdown
+    std::atomic<uint16_t> detectedShutdowns{0};
+    std::atomic<float> estimatedAltitude{0};
+    std::atomic<float> estimatedMass{0};
+
+    // Last update timestamp of mea
+    uint64_t lastUpdateTimestamp = 0;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("MEA");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/MEAController/MEAControllerData.h b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..a531d8d99295f7b99567331ccb86dfb6d76ae6fc
--- /dev/null
+++ b/src/boards/Main/StateMachines/MEAController/MEAControllerData.h
@@ -0,0 +1,63 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+enum class MEAControllerState : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    READY,
+    ARMED,
+    SHADOW_MODE,
+    ACTIVE,
+    ACTIVE_DISARMED,
+    END
+};
+
+struct MEAControllerStatus
+{
+    uint64_t timestamp         = 0;
+    float estimatedMass        = 0;
+    float estimatedApogee      = 0;
+    uint16_t detectedShutdowns = 0;
+    MEAControllerState state   = MEAControllerState::UNINIT;
+
+    static std::string header()
+    {
+        return "timestamp,estimatedMass,estimatedApogee,detectedShutdowns,"
+               "state\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << estimatedMass << "," << estimatedApogee << ","
+           << (int)detectedShutdowns << "," << (int)state << "\n";
+    }
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e63149930c70f42e66191bc16de4bb63ff1c8b5
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -0,0 +1,384 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Configs/NASConfig.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <events/EventBroker.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace std;
+using namespace Common;
+namespace Main
+{
+NASController::NASController(TaskScheduler* sched)
+    : FSM(&NASController::state_idle), nas(NASConfig::config), scheduler(sched)
+{
+    // Subscribe the class to the topics
+    EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+    // Setup the NAS
+    Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+    // Create the initial quaternion
+    Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+
+    // Set the initial quaternion inside the matrix
+    x(6) = q(0);
+    x(7) = q(1);
+    x(8) = q(2);
+    x(9) = q(3);
+
+    // Set the NAS x matrix
+    nas.setX(x);
+
+    // Set the referenced values for the correct place on earth
+    nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+}
+
+bool NASController::start()
+{
+    // Add the task to the scheduler
+    size_t result = scheduler->addTask(bind(&NASController::update, this),
+                                       NASConfig::UPDATE_PERIOD,
+                                       TaskScheduler::Policy::RECOVER);
+
+    return ActiveObject::start() && result != 0;
+}
+
+void NASController::update()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Update the NAS state only if the FSM is active
+    if (this->testState(&NASController::state_active))
+    {
+        // Get the IMU data
+        RotatedIMUData imuData = modules.get<Sensors>()->getIMULastSample();
+        UBXGPSData gpsData     = modules.get<Sensors>()->getGPSLastSample();
+
+        // Get barometer data
+        PressureData staticPressure =
+            modules.get<Sensors>()->getStaticPressure1LastSample();
+
+        // Compute the norm for acceleration validity
+        Vector3f accVector     = static_cast<AccelerometerData>(imuData);
+        float accelerationNorm = accVector.norm();
+
+        // NAS prediction
+        nas.predictGyro(imuData);
+        nas.predictAcc(imuData);
+
+        // NAS correction
+        nas.correctMag(imuData);
+        nas.correctGPS(gpsData);
+        nas.correctBaro(staticPressure.pressure);
+
+        // Correct with accelerometer if the acceleration is in specs
+        if (accelerationValid)
+        {
+            nas.correctAcc(imuData);
+        }
+
+        // Check boundaries
+        if ((accelerationNorm <
+                 (Constants::g + (NASConfig::ACCELERATION_THRESHOLD) / 2) &&
+             accelerationNorm >
+                 (Constants::g - (NASConfig::ACCELERATION_THRESHOLD) / 2)))
+        {
+            if (!accelerationValid)
+            {
+                accSampleAfterSpike++;
+            }
+        }
+        else
+        {
+            accelerationValid   = false;
+            accSampleAfterSpike = 0;
+        }
+
+        // Enable the accelerometer after a fixed amount of good samples
+        if (accSampleAfterSpike > NASConfig::ACCELERATION_THRESHOLD_SAMPLE)
+        {
+            accSampleAfterSpike = 0;
+            accelerationValid   = true;
+        }
+
+        Logger::getInstance().log(nas.getState());
+
+        // printf("/w%fwa%fab%fbc%fc/\n", getNasState().qw, getNasState().qx,
+        //        getNasState().qy, getNasState().qz);
+    }
+}
+
+void NASController::calibrate()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    Vector3f acceleration  = Vector3f::Zero();
+    Vector3f magneticField = Vector3f::Zero();
+    Stats pressure;
+
+    for (int i = 0; i < NASConfig::CALIBRATION_SAMPLES_COUNT; i++)
+    {
+        // IMU
+        LSM6DSRXData imuData = modules.get<Sensors>()->getLSM6DSRXLastSample();
+        acceleration += Vector3f(imuData.accelerationX, imuData.accelerationY,
+                                 imuData.accelerationZ);
+
+        // Magnetometer
+        LIS2MDLData magData = modules.get<Sensors>()->getLIS2MDLLastSample();
+        magneticField +=
+            Vector3f(magData.magneticFieldX, magData.magneticFieldY,
+                     magData.magneticFieldZ);
+
+        // Static pressure barometer
+        PressureData barometerData =
+            modules.get<Sensors>()->getStaticPressure1LastSample();
+        pressure.add(barometerData.pressure);
+
+        miosix::Thread::sleep(NASConfig::CALIBRATION_SLEEP_TIME);
+    }
+
+    // Normalize the data
+    acceleration /= NASConfig::CALIBRATION_SAMPLES_COUNT;
+    magneticField /= NASConfig::CALIBRATION_SAMPLES_COUNT;
+    acceleration.normalize();
+    magneticField.normalize();
+
+    // Use the triad algorithm to estimate the initial orientation
+    StateInitializer state;
+    state.triad(acceleration, magneticField, ReferenceConfig::nedMag);
+
+    // Set the pressure reference using an already existing reference values
+    ReferenceValues reference = nas.getReferenceValues();
+    reference.refPressure     = pressure.getStats().mean;
+
+    // Set reference altitude using barometric measure
+    reference.refAltitude = Aeroutils::relAltitude(pressure.getStats().mean);
+
+    // If in this moment the GPS has fix i use that position as starting
+    UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample();
+    if (gps.fix != 0)
+    {
+        // We don't set the altitude with the GPS because of not precise
+        // measurements
+        reference.refLatitude  = gps.latitude;
+        reference.refLongitude = gps.longitude;
+    }
+
+    // Update the algorithm reference values
+    {
+        miosix::PauseKernelLock lock;
+        nas.setX(state.getInitX());
+        nas.setReferenceValues(reference);
+    }
+
+    EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+}
+
+void NASController::setCoordinates(Vector2f position)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = nas.getReferenceValues();
+    reference.refLatitude     = position[0];
+    reference.refLongitude    = position[1];
+    nas.setReferenceValues(reference);
+}
+
+void NASController::setOrientation(float yaw, float pitch, float roll)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    Matrix<float, 13, 1> x = nas.getX();
+
+    // Set quaternions
+    Vector4f q = SkyQuaternion::eul2quat({yaw, pitch, roll});
+    x(6)       = q(0);
+    x(7)       = q(1);
+    x(8)       = q(2);
+    x(9)       = q(3);
+
+    nas.setX(x);
+}
+
+void NASController::setReferenceAltitude(float altitude)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = nas.getReferenceValues();
+    reference.refAltitude     = altitude;
+    nas.setReferenceValues(reference);
+}
+
+void NASController::setReferenceTemperature(float temperature)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = nas.getReferenceValues();
+
+    // The temperature is in degrees, converted in kelvin
+    reference.refTemperature = temperature + 273.15f;
+    nas.setReferenceValues(reference);
+}
+
+void NASController::setReferenceValues(const ReferenceValues reference)
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    nas.setReferenceValues(reference);
+}
+
+NASControllerStatus NASController::getStatus()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return status;
+}
+
+NASState NASController::getNasState()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return nas.getState();
+}
+
+ReferenceValues NASController::getReferenceValues()
+{
+    // Need to pause the kernel
+    miosix::PauseKernelLock l;
+    return nas.getReferenceValues();
+}
+
+void NASController::state_idle(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(NASControllerState::IDLE);
+        }
+        case NAS_CALIBRATE:
+        {
+            return transition(&NASController::state_calibrating);
+        }
+    }
+}
+
+void NASController::state_calibrating(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            logStatus(NASControllerState::CALIBRATING);
+
+            // Calibrate the NAS
+            calibrate();
+            break;
+        }
+        case NAS_READY:
+        {
+            return transition(&NASController::state_ready);
+        }
+    }
+}
+
+void NASController::state_ready(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(NASControllerState::READY);
+        }
+        case NAS_CALIBRATE:
+        {
+            return transition(&NASController::state_calibrating);
+        }
+        case NAS_FORCE_START:
+        case FLIGHT_ARMED:
+        {
+            return transition(&NASController::state_active);
+        }
+    }
+}
+
+void NASController::state_active(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(NASControllerState::ACTIVE);
+        }
+        case FLIGHT_LANDING_DETECTED:
+        {
+            return transition(&NASController::state_end);
+        }
+        case NAS_FORCE_STOP:
+        case FLIGHT_DISARMED:
+        {
+            return transition(&NASController::state_ready);
+        }
+    }
+}
+
+void NASController::state_end(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(NASControllerState::END);
+        }
+    }
+}
+
+void NASController::logStatus(NASControllerState state)
+{
+    {
+        miosix::PauseKernelLock lock;
+        // Update the current FSM state
+        status.timestamp = TimestampTimer::getTimestamp();
+        status.state     = state;
+    }
+
+    // Log the status
+    Logger::getInstance().log(status);
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
new file mode 100644
index 0000000000000000000000000000000000000000..01658f4122ec16ae1acf892d377e6581747c41c3
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -0,0 +1,94 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/NAS/NAS.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "NASControllerData.h"
+
+namespace Main
+{
+class NASController : public Boardcore::FSM<NASController>,
+                      public Boardcore::Module
+{
+public:
+    NASController(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the FSM thread and adds an update function into the
+     * scheduler
+     */
+    bool start() override;
+
+    // NAS FSM called methods
+    void update();
+    void calibrate();
+
+    // NAS setters
+    void setCoordinates(Eigen::Vector2f position);
+    void setOrientation(float yaw, float pitch, float roll);
+    void setReferenceAltitude(float altitude);
+    void setReferenceTemperature(float temperature);
+    void setReferenceValues(const Boardcore::ReferenceValues reference);
+
+    // NAS Getters
+    NASControllerStatus getStatus();
+    Boardcore::NASState getNasState();
+    Boardcore::ReferenceValues getReferenceValues();
+
+    // FSM states
+    void state_idle(const Boardcore::Event& event);
+    void state_calibrating(const Boardcore::Event& event);
+    void state_ready(const Boardcore::Event& event);
+    void state_active(const Boardcore::Event& event);
+    void state_end(const Boardcore::Event& event);
+
+private:
+    /**
+     * @brief Logs the NAS status updating the FSM state
+     * @param state The current FSM state
+     */
+    void logStatus(NASControllerState state);
+
+    // Controller state machine status
+    NASControllerStatus status;
+    Boardcore::NAS nas;
+
+    // Scheduler to be used for update function
+    Boardcore::TaskScheduler* scheduler;
+
+    // User set (or triac set) initial orientation
+    Eigen::Vector3f initialOrientation;
+
+    // Acceleration correction parameters
+    bool accelerationValid           = true;
+    unsigned int accSampleAfterSpike = 0;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS");
+};
+}  // namespace Main
diff --git a/src/boards/Main/StateMachines/NASController/NASControllerData.h b/src/boards/Main/StateMachines/NASController/NASControllerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..559722a552f163863d269ca81edc517b9d07b5d8
--- /dev/null
+++ b/src/boards/Main/StateMachines/NASController/NASControllerData.h
@@ -0,0 +1,56 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace Main
+{
+
+enum class NASControllerState : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    CALIBRATING,
+    READY,
+    ACTIVE,
+    END
+};
+
+struct NASControllerStatus
+{
+    long long timestamp      = 0;
+    NASControllerState state = NASControllerState::UNINIT;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+
+}  // namespace Main
diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc612e4056814826b735d1358b189efade719d45
--- /dev/null
+++ b/src/boards/Main/TMRepository/TMRepository.cpp
@@ -0,0 +1,736 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#include <Main/Actuators/Actuators.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Configs/RadioConfig.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ABKController/ABKController.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/Deployment/Deployment.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+namespace Main
+{
+mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
+                                             uint8_t seq)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    mavlink_message_t msg;
+
+    switch (tmId)
+    {
+        case SystemTMList::MAV_SYS_ID:
+        {
+            mavlink_sys_tm_t tm;
+
+            tm.timestamp       = TimestampTimer::getTimestamp();
+            tm.logger          = Logger::getInstance().isStarted();
+            tm.board_scheduler = modules.get<BoardScheduler>()->isStarted();
+            tm.event_broker    = EventBroker::getInstance().isRunning();
+            tm.radio           = modules.get<Radio>()->isStarted();
+            tm.sensors         = modules.get<Sensors>()->isStarted();
+            tm.pin_observer    = modules.get<PinHandler>()->isStarted();
+
+            mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_LOGGER_ID:
+        {
+            mavlink_logger_tm_t tm;
+
+            // Get the logger stats
+            LoggerStats stats = Logger::getInstance().getStats();
+
+            tm.timestamp          = stats.timestamp;
+            tm.average_write_time = stats.averageWriteTime;
+            tm.buffers_filled     = stats.buffersFilled;
+            tm.buffers_written    = stats.buffersWritten;
+            tm.dropped_samples    = stats.droppedSamples;
+            tm.last_write_error   = stats.lastWriteError;
+            tm.log_number         = stats.logNumber;
+            tm.max_write_time     = stats.maxWriteTime;
+            tm.queued_samples     = stats.queuedSamples;
+            tm.too_large_samples  = stats.tooLargeSamples;
+            tm.writes_failed      = stats.writesFailed;
+
+            mavlink_msg_logger_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                         RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_MAVLINK_STATS:
+        {
+            mavlink_mavlink_stats_tm_t tm;
+
+            // Get the mavlink stats
+            MavlinkStatus stats = modules.get<Radio>()->mavDriver->getStatus();
+
+            tm.timestamp               = stats.timestamp;
+            tm.n_send_queue            = stats.nSendQueue;
+            tm.max_send_queue          = stats.maxSendQueue;
+            tm.n_send_errors           = stats.nSendErrors;
+            tm.msg_received            = stats.mavStats.msg_received;
+            tm.buffer_overrun          = stats.mavStats.buffer_overrun;
+            tm.parse_error             = stats.mavStats.parse_error;
+            tm.parse_state             = stats.mavStats.parse_state;
+            tm.packet_idx              = stats.mavStats.packet_idx;
+            tm.current_rx_seq          = stats.mavStats.current_rx_seq;
+            tm.current_tx_seq          = stats.mavStats.current_tx_seq;
+            tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count;
+            tm.packet_rx_drop_count    = stats.mavStats.packet_rx_drop_count;
+
+            mavlink_msg_mavlink_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                                RadioConfig::MAV_COMP_ID, &msg,
+                                                &tm);
+            break;
+        }
+        case SystemTMList::MAV_NAS_ID:
+        {
+            mavlink_nas_tm_t tm;
+
+            // Get the current NAS state
+            NASState state = modules.get<NASController>()->getNasState();
+            ReferenceValues ref =
+                modules.get<NASController>()->getReferenceValues();
+
+            tm.timestamp = state.timestamp;
+            tm.state     = static_cast<uint8_t>(
+                modules.get<NASController>()->getStatus().state);
+            tm.nas_n           = state.n;
+            tm.nas_e           = state.e;
+            tm.nas_d           = state.d;
+            tm.nas_vn          = state.vn;
+            tm.nas_ve          = state.ve;
+            tm.nas_vd          = state.vd;
+            tm.nas_qx          = state.qx;
+            tm.nas_qy          = state.qy;
+            tm.nas_qz          = state.qz;
+            tm.nas_qw          = state.qw;
+            tm.nas_bias_x      = state.bx;
+            tm.nas_bias_y      = state.by;
+            tm.nas_bias_z      = state.bz;
+            tm.ref_pressure    = ref.refPressure;
+            tm.ref_temperature = ref.refTemperature;
+            tm.ref_latitude    = ref.refLatitude;
+            tm.ref_longitude   = ref.refLongitude;
+
+            mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_ADA_ID:
+        {
+            mavlink_ada_tm_t tm;
+
+            // Get the current ADA state
+            ADAState state = modules.get<ADAController>()->getADAState();
+            ReferenceValues ref =
+                modules.get<ADAController>()->getReferenceValues();
+
+            tm.timestamp = state.timestamp;
+            tm.state     = static_cast<uint8_t>(
+                modules.get<ADAController>()->getStatus().state);
+            tm.kalman_x0       = state.x0;
+            tm.kalman_x1       = state.x1;
+            tm.kalman_x2       = state.x2;
+            tm.vertical_speed  = state.verticalSpeed;
+            tm.msl_altitude    = state.mslAltitude;
+            tm.msl_pressure    = ref.mslPressure;
+            tm.msl_temperature = ref.mslTemperature - 273.15f;
+            tm.ref_altitude    = ref.refAltitude;
+            tm.ref_temperature = ref.refTemperature - 273.15f;
+            tm.ref_pressure    = ref.refPressure;
+            tm.dpl_altitude    = 0;
+
+            mavlink_msg_ada_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_FLIGHT_ID:
+        {
+            mavlink_rocket_flight_tm_t tm;
+
+            tm.timestamp = TimestampTimer::getTimestamp();
+
+            // Last samples
+            LSM6DSRXData lsm6dsrx =
+                modules.get<Sensors>()->getLSM6DSRXLastSample();
+            LPS28DFWData lps28dfw1 =
+                modules.get<Sensors>()->getLPS28DFW_1LastSample();
+            LIS2MDLData lis2mdl =
+                modules.get<Sensors>()->getLIS2MDLLastSample();
+            PressureData staticPressure =
+                modules.get<Sensors>()->getStaticPressure1LastSample();
+            PressureData deploymentPressure =
+                modules.get<Sensors>()->getDeploymentPressureLastSample();
+            UBXGPSData gps = modules.get<Sensors>()->getGPSLastSample();
+
+            // NAS state
+            NASState nasState = modules.get<NASController>()->getNasState();
+
+            // ADA state
+            ADAState adaState = modules.get<ADAController>()->getADAState();
+
+            // State machines
+            tm.mea_state = static_cast<uint8_t>(
+                modules.get<MEAController>()->getStatus().state);
+            tm.ada_state = static_cast<uint8_t>(
+                modules.get<ADAController>()->getStatus().state);
+            tm.nas_state = static_cast<uint8_t>(
+                modules.get<NASController>()->getStatus().state);
+            tm.abk_state = static_cast<uint8_t>(
+                modules.get<ABKController>()->getStatus().state);
+            tm.fmm_state = static_cast<uint8_t>(
+                modules.get<FlightModeManager>()->getStatus().state);
+            tm.dpl_state = static_cast<uint8_t>(
+                modules.get<Deployment>()->getStatus().state);
+
+            // Pressures
+            tm.pressure_ada    = adaState.x0;
+            tm.pressure_digi   = lps28dfw1.pressure;
+            tm.pressure_dpl    = deploymentPressure.pressure;
+            tm.pressure_static = staticPressure.pressure;
+
+            // Altitude agl
+            tm.altitude_agl = -nasState.d;
+
+            // IMU
+            tm.acc_x  = lsm6dsrx.accelerationX;
+            tm.acc_y  = lsm6dsrx.accelerationY;
+            tm.acc_z  = lsm6dsrx.accelerationZ;
+            tm.gyro_x = lsm6dsrx.angularSpeedX;
+            tm.gyro_y = lsm6dsrx.angularSpeedY;
+            tm.gyro_z = lsm6dsrx.angularSpeedZ;
+
+            // Magnetometer
+            tm.mag_x = lis2mdl.magneticFieldX;
+            tm.mag_y = lis2mdl.magneticFieldY;
+            tm.mag_z = lis2mdl.magneticFieldZ;
+
+            // GPS
+            tm.gps_fix = gps.fix;
+            tm.gps_lat = gps.latitude;
+            tm.gps_lon = gps.longitude;
+            tm.gps_alt = gps.height;
+
+            // NAS
+            tm.nas_n      = nasState.n;
+            tm.nas_e      = nasState.e;
+            tm.nas_d      = nasState.d;
+            tm.nas_vn     = nasState.vn;
+            tm.nas_ve     = nasState.ve;
+            tm.nas_vd     = nasState.vd;
+            tm.nas_qx     = nasState.qx;
+            tm.nas_qy     = nasState.qy;
+            tm.nas_qz     = nasState.qz;
+            tm.nas_qw     = nasState.qw;
+            tm.nas_bias_x = nasState.bx;
+            tm.nas_bias_y = nasState.by;
+            tm.nas_bias_z = nasState.bz;
+
+            // ADA
+            tm.ada_vert_speed = adaState.verticalSpeed;
+
+            // ABK
+            tm.abk_angle = modules.get<Actuators>()->getServoPosition(
+                ServosList::AIR_BRAKES_SERVO);
+
+            // Pins
+            tm.pin_launch = modules.get<PinHandler>()
+                                ->getPinData(PinHandler::PinList::LAUNCH_PIN)
+                                .lastState;
+            tm.pin_nosecone =
+                modules.get<PinHandler>()
+                    ->getPinData(PinHandler::PinList::NOSECONE_PIN)
+                    .lastState;
+            tm.pin_expulsion =
+                modules.get<PinHandler>()
+                    ->getPinData(PinHandler::PinList::PIN_EXPULSION)
+                    .lastState;
+
+            // Board status
+            tm.battery_voltage = modules.get<Sensors>()
+                                     ->getBatteryVoltageLastSample()
+                                     .batVoltage;
+            tm.cam_battery_voltage = modules.get<Sensors>()
+                                         ->getCamBatteryVoltageLastSample()
+                                         .batVoltage;
+            tm.current_consumption =
+                modules.get<Sensors>()->getCurrentLastSample().current;
+            tm.temperature  = lps28dfw1.temperature;
+            tm.logger_error = Logger::getInstance().getStats().lastWriteError;
+            tm.cutter_presence =
+                modules.get<PinHandler>()
+                    ->getPinData(PinHandler::PinList::CUTTER_PRESENCE)
+                    .lastState;
+
+            // Pitot CAN sensor
+            tm.airspeed_pitot =
+                modules.get<Sensors>()->getPitotLastSample().airspeed;
+
+            mavlink_msg_rocket_flight_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                                RadioConfig::MAV_COMP_ID, &msg,
+                                                &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_STATS_ID:
+        {
+            mavlink_rocket_stats_tm_t tm =
+                modules.get<FlightStatsRecorder>()->getStats();
+
+            tm.cpu_load  = CpuMeter::getCpuStats().mean;
+            tm.free_heap = CpuMeter::getCpuStats().freeHeap;
+
+            mavlink_msg_rocket_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                               RadioConfig::MAV_COMP_ID, &msg,
+                                               &tm);
+            break;
+        }
+        case SystemTMList::MAV_FSM_ID:
+        {
+            mavlink_fsm_tm_t tm;
+
+            tm.timestamp = TimestampTimer::getTimestamp();
+            tm.abk_state = static_cast<uint8_t>(
+                modules.get<ABKController>()->getStatus().state);
+            tm.ada_state = static_cast<uint8_t>(
+                modules.get<ADAController>()->getStatus().state);
+            tm.dpl_state = static_cast<uint8_t>(
+                modules.get<Deployment>()->getStatus().state);
+            tm.fmm_state = static_cast<uint8_t>(
+                modules.get<FlightModeManager>()->getStatus().state);
+            tm.nas_state = static_cast<uint8_t>(
+                modules.get<NASController>()->getStatus().state);
+
+            mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_MOTOR_ID:
+        {
+            mavlink_motor_tm_t tm;
+
+            tm.timestamp            = TimestampTimer::getTimestamp();
+            tm.bottom_tank_pressure = modules.get<Sensors>()
+                                          ->getBottomTankPressureLastSample()
+                                          .pressure;
+            tm.combustion_chamber_pressure =
+                modules.get<Sensors>()->getCCPressureLastSample().pressure;
+            tm.tank_temperature = modules.get<Sensors>()
+                                      ->getTankTemperatureLastSample()
+                                      .temperature;
+            // clang-format off
+            tm.main_valve_state    = modules.get<Actuators>()->getServoPosition(
+                                         ServosList::MAIN_VALVE) > 0.3 ? 1 : 0;
+            tm.venting_valve_state = modules.get<Actuators>()->getServoPosition(
+                                         ServosList::VENTING_VALVE) > 0.3 ? 1 : 0;
+            // clang-format on
+
+            tm.top_tank_pressure =
+                modules.get<Sensors>()->getTopTankPressureLastSample().pressure;
+            tm.battery_voltage =
+                modules.get<Sensors>()->getMotorBatteryVoltage().batVoltage;
+            tm.current_consumption =
+                modules.get<Sensors>()->getMotorCurrent().current;
+
+            mavlink_msg_motor_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                        RadioConfig::MAV_COMP_ID, &msg, &tm);
+            break;
+        }
+        default:
+        {
+            // Send by default the nack message
+            mavlink_nack_tm_t nack;
+
+            nack.recv_msgid = msgId;
+            nack.seq_ack    = seq;
+
+            LOG_ERR(logger, "Unknown message id: {}", tmId);
+            mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                       RadioConfig::MAV_COMP_ID, &msg, &nack);
+
+            break;
+        }
+    }
+    return msg;
+}
+
+mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
+                                              uint8_t msgId, uint8_t seq)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    mavlink_message_t msg;
+
+    switch (sensorId)
+    {
+        case MAV_GPS_ID:
+        {
+            mavlink_gps_tm_t tm;
+
+            UBXGPSData data = modules.get<Sensors>()->getGPSLastSample();
+
+            tm.fix          = data.fix;
+            tm.height       = data.height;
+            tm.latitude     = data.latitude;
+            tm.longitude    = data.longitude;
+            tm.n_satellites = data.satellites;
+            strcpy(tm.sensor_name, "UBXGPS");
+            tm.speed     = data.speed;
+            tm.timestamp = data.gpsTimestamp;
+            tm.track     = data.track;
+            tm.vel_down  = data.velocityDown;
+            tm.vel_east  = data.velocityEast;
+            tm.vel_north = data.velocityNorth;
+
+            mavlink_msg_gps_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_ADS_ID:
+        {
+            mavlink_adc_tm_t tm;
+
+            ADS131M08Data data =
+                modules.get<Sensors>()->getADS131M08LastSample();
+
+            tm.channel_0 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
+            tm.channel_1 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
+            tm.channel_2 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
+            tm.channel_3 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
+            tm.channel_4 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
+            tm.channel_5 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
+            tm.channel_6 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
+            tm.channel_7 =
+                data.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
+            tm.timestamp = data.timestamp;
+
+            strcpy(tm.sensor_name, "ADS131M08");
+
+            mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_CURRENT_SENSE_ID:
+        {
+            mavlink_current_tm_t tm;
+
+            CurrentData data = modules.get<Sensors>()->getCurrentLastSample();
+
+            tm.current   = data.current;
+            tm.timestamp = data.currentTimestamp;
+            strcpy(tm.sensor_name, "CURRENT");
+
+            mavlink_msg_current_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                          RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_DPL_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData data =
+                modules.get<Sensors>()->getDeploymentPressureLastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "DPL_PRESSURE");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_STATIC_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData data =
+                modules.get<Sensors>()->getStaticPressure1LastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "STATIC_PRESSURE");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_BATTERY_VOLTAGE_ID:
+        {
+            mavlink_voltage_tm_t tm;
+
+            BatteryVoltageSensorData data =
+                modules.get<Sensors>()->getBatteryVoltageLastSample();
+
+            tm.voltage   = data.batVoltage;
+            tm.timestamp = data.voltageTimestamp;
+            strcpy(tm.sensor_name, "BATTERY_VOLTAGE");
+
+            mavlink_msg_voltage_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                          RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_TANK_TOP_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData data =
+                modules.get<Sensors>()->getTopTankPressureLastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "TOP_TANK");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_TANK_BOTTOM_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData data =
+                modules.get<Sensors>()->getBottomTankPressureLastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "BOTTOM_TANK");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_COMBUSTION_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData data =
+                modules.get<Sensors>()->getCCPressureLastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "COMBUSTION_CHAMBER");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_TANK_TEMP_ID:
+        {
+            mavlink_temp_tm_t tm;
+
+            TemperatureData data =
+                modules.get<Sensors>()->getTankTemperatureLastSample();
+
+            tm.temperature = data.temperature;
+            tm.timestamp   = data.temperatureTimestamp;
+            strcpy(tm.sensor_name, "TANK_TEMPERATURE");
+
+            mavlink_msg_temp_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                       RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_LIS2MDL_ID:
+        {
+            mavlink_imu_tm_t tm;
+
+            LIS2MDLData data = modules.get<Sensors>()->getLIS2MDLLastSample();
+
+            tm.acc_x  = 0;
+            tm.acc_y  = 0;
+            tm.acc_z  = 0;
+            tm.gyro_x = 0;
+            tm.gyro_y = 0;
+            tm.gyro_z = 0;
+
+            tm.mag_x     = data.magneticFieldX;
+            tm.mag_y     = data.magneticFieldY;
+            tm.mag_z     = data.magneticFieldZ;
+            tm.timestamp = data.magneticFieldTimestamp;
+            strcpy(tm.sensor_name, "LIS2MDL");
+
+            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_LPS28DFW_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            LPS28DFWData data =
+                modules.get<Sensors>()->getLPS28DFW_1LastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "LPS28DFW");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_LSM6DSRX_ID:
+        {
+            mavlink_imu_tm_t tm;
+
+            LSM6DSRXData data = modules.get<Sensors>()->getLSM6DSRXLastSample();
+
+            tm.mag_x     = 0;
+            tm.mag_y     = 0;
+            tm.mag_z     = 0;
+            tm.acc_x     = data.accelerationX;
+            tm.acc_y     = data.accelerationY;
+            tm.acc_z     = data.accelerationZ;
+            tm.gyro_x    = data.angularSpeedX;
+            tm.gyro_y    = data.angularSpeedY;
+            tm.gyro_z    = data.angularSpeedZ;
+            tm.timestamp = data.accelerationTimestamp;
+            strcpy(tm.sensor_name, "LSM6DSRX");
+
+            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_H3LIS331DL_ID:
+        {
+            mavlink_imu_tm_t tm;
+
+            H3LIS331DLData data =
+                modules.get<Sensors>()->getH3LIS331DLLastSample();
+
+            tm.mag_x     = 0;
+            tm.mag_y     = 0;
+            tm.mag_z     = 0;
+            tm.gyro_x    = 0;
+            tm.gyro_y    = 0;
+            tm.gyro_z    = 0;
+            tm.acc_x     = data.accelerationX;
+            tm.acc_y     = data.accelerationY;
+            tm.acc_z     = data.accelerationZ;
+            tm.timestamp = data.accelerationTimestamp;
+            strcpy(tm.sensor_name, "H3LIS331DL");
+
+            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        case MAV_LPS22DF_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            LPS22DFData data = modules.get<Sensors>()->getLPS22DFLastSample();
+
+            tm.pressure  = data.pressure;
+            tm.timestamp = data.pressureTimestamp;
+            strcpy(tm.sensor_name, "LPS22DF");
+
+            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+            break;
+        }
+        default:
+        {
+            // Send by default the nack message
+            mavlink_nack_tm_t nack;
+
+            nack.recv_msgid = msgId;
+            nack.seq_ack    = seq;
+
+            LOG_ERR(logger, "Unknown sensor id: {}", sensorId);
+            mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                       RadioConfig::MAV_COMP_ID, &msg, &nack);
+
+            break;
+        }
+    }
+
+    return msg;
+}
+
+mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId,
+                                            uint8_t seq)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    mavlink_message_t msg;
+
+    if (servoId == AIR_BRAKES_SERVO || servoId == EXPULSION_SERVO ||
+        servoId == MAIN_VALVE || servoId == VENTING_VALVE)
+    {
+        mavlink_servo_tm_t tm;
+        tm.servo_id       = servoId;
+        tm.servo_position = modules.get<Actuators>()->getServoPosition(servoId);
+
+        mavlink_msg_servo_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                    RadioConfig::MAV_COMP_ID, &msg, &tm);
+    }
+    else
+    {
+        mavlink_nack_tm_t nack;
+        nack.recv_msgid = msgId;
+        nack.seq_ack    = seq;
+
+        mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+                                   RadioConfig::MAV_COMP_ID, &msg, &nack);
+    }
+
+    return msg;
+}
+}  // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/TMRepository/TMRepository.h b/src/boards/Main/TMRepository/TMRepository.h
new file mode 100644
index 0000000000000000000000000000000000000000..9cb72f0acb6185aa1ffc34744864929837c50e18
--- /dev/null
+++ b/src/boards/Main/TMRepository/TMRepository.h
@@ -0,0 +1,52 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#pragma once
+
+#include <common/Mavlink.h>
+#include <diagnostic/PrintLogger.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace Main
+{
+class TMRepository : public Boardcore::Module
+{
+public:
+    inline TMRepository() {}
+
+    // Packs the telemetry at system level (E.g. logger telemetry..)
+    mavlink_message_t packSystemTm(SystemTMList tmId, uint8_t msgId,
+                                   uint8_t seq);
+
+    // Packs the telemetry at sensors level (E.g. pressure sensors..)
+    mavlink_message_t packSensorsTm(SensorsTMList sensorId, uint8_t msgId,
+                                    uint8_t seq);
+
+    // Packs the telemetry about servo positions states
+    mavlink_message_t packServoTm(ServosList servoId, uint8_t msgId,
+                                  uint8_t seq);
+
+private:
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("TMRepository");
+};
+}  // namespace Main
\ No newline at end of file
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa575195a0d393861dfcf3bacd6f0c9dbeed70c5
--- /dev/null
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -0,0 +1,319 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Main/Actuators/Actuators.h>
+#include <Main/AltitudeTrigger/AltitudeTrigger.h>
+#include <Main/BoardScheduler.h>
+#include <Main/Buses.h>
+#include <Main/CanHandler/CanHandler.h>
+#include <Main/FlightStatsRecorder/FlightStatsRecorder.h>
+#include <Main/PinHandler/PinHandler.h>
+#include <Main/Radio/Radio.h>
+#include <Main/Sensors/Sensors.h>
+#include <Main/StateMachines/ABKController/ABKController.h>
+#include <Main/StateMachines/ADAController/ADAController.h>
+#include <Main/StateMachines/Deployment/Deployment.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Main/StateMachines/MEAController/MEAController.h>
+#include <Main/StateMachines/NASController/NASController.h>
+#include <Main/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <diagnostic/StackLogger.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace Main;
+using namespace Common;
+
+int main()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Overall status, if at some point it becomes false, there is a problem
+    // somewhere
+    bool initResult    = true;
+    PrintLogger logger = Logging::getLogger("main");
+
+    // Create modules
+    BoardScheduler* scheduler = new BoardScheduler();
+    Buses* buses              = new Buses();
+    Sensors* sensors =
+        new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+    NASController* nas =
+        new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    ADAController* ada =
+        new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    TMRepository* tmRepo = new TMRepository();
+    CanHandler* canHandler =
+        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    FlightModeManager* fmm = new FlightModeManager();
+    Actuators* actuators =
+        new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY));
+    Deployment* dpl        = new Deployment();
+    PinHandler* pinHandler = new PinHandler();
+    AltitudeTrigger* altitudeTrigger =
+        new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    MEAController* mea =
+        new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    ABKController* abk =
+        new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX));
+    FlightStatsRecorder* recorder =
+        new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY));
+
+    // Insert modules
+    if (!modules.insert<BoardScheduler>(scheduler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Board Scheduler module");
+    }
+
+    if (!modules.insert<Buses>(buses))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Buses module");
+    }
+
+    if (!modules.insert<Sensors>(sensors))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Sensor module");
+    }
+
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Actuators module");
+    }
+
+    if (!modules.insert<Deployment>(dpl))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the DPL module");
+    }
+
+    if (!modules.insert<NASController>(nas))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the NAS module");
+    }
+
+    if (!modules.insert<ADAController>(ada))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the ADA module");
+    }
+
+    if (!modules.insert<MEAController>(mea))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the MEA module");
+    }
+
+    if (!modules.insert<ABKController>(abk))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the ABK controller module");
+    }
+
+    if (!modules.insert<Radio>(radio))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Radio module");
+    }
+
+    if (!modules.insert<FlightModeManager>(fmm))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the FMM module");
+    }
+
+    if (!modules.insert<TMRepository>(tmRepo))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the TMRepository module");
+    }
+
+    if (!modules.insert<CanHandler>(canHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the CanHandler module");
+    }
+
+    if (!modules.insert<PinHandler>(pinHandler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the PinHandler module");
+    }
+
+    if (!modules.insert<AltitudeTrigger>(altitudeTrigger))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Altitude Trigger module");
+    }
+
+    if (!modules.insert<FlightStatsRecorder>(recorder))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the Flight Stats Recorder module");
+    }
+
+    // Start modules
+    if (!Logger::getInstance().testSDCard())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Logger module");
+    }
+
+    if (!EventBroker::getInstance().start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the EventBroker module");
+    }
+
+    if (!modules.get<BoardScheduler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Board Scheduler module");
+    }
+
+    if (!modules.get<Actuators>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Actuators module");
+    }
+
+    if (!modules.get<Deployment>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Deployment module");
+    }
+
+    if (!modules.get<Sensors>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Sensors module");
+    }
+
+    if (!modules.get<NASController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the NAS module");
+    }
+
+    if (!modules.get<ADAController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ADA module");
+    }
+
+    if (!modules.get<MEAController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the MEA module");
+    }
+
+    if (!modules.get<ABKController>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ABK controller module");
+    }
+
+    if (!modules.get<FlightModeManager>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the FMM module");
+    }
+
+    if (!modules.get<Radio>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Radio module");
+    }
+
+    if (!modules.get<CanHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the CanHandler module");
+    }
+
+    if (!modules.get<PinHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the PinHandler module");
+    }
+
+    if (!modules.get<AltitudeTrigger>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Altitude Trigger module");
+    }
+
+    if (!modules.get<FlightStatsRecorder>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Flight Stats Recorder module");
+    }
+
+    // Log all the events
+    EventSniffer sniffer(
+        EventBroker::getInstance(), TOPICS_LIST,
+        [](uint8_t event, uint8_t topic)
+        {
+            EventData ev{TimestampTimer::getTimestamp(), event, topic};
+            Logger::getInstance().log(ev);
+        });
+
+    // Check the init result and launch an event
+    if (initResult)
+    {
+        // Post OK
+        EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+
+        // Set the LED status
+        miosix::led1On();
+    }
+    else
+    {
+        EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM);
+        LOG_ERR(logger, "Failed to initialize");
+    }
+
+    // Periodic statistics
+    while (true)
+    {
+        Thread::sleep(1000);
+        Logger::getInstance().log(CpuMeter::getCpuStats());
+        CpuMeter::resetCpuStats();
+        StackLogger::getInstance().log();
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/scripts/EventDumper/EventDumper.cpp b/src/scripts/EventDumper/EventDumper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fb6d768353e40f6f25ed01cf497920e8b3713b5
--- /dev/null
+++ b/src/scripts/EventDumper/EventDumper.cpp
@@ -0,0 +1,42 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <common/Events.h>
+
+#include <iostream>
+#include <string>
+
+using namespace std;
+
+int main()
+{
+    // Scan all the indices and print their correspondent name
+    for (int i = Boardcore::EV_FIRST_CUSTOM; i < 256; i++)
+    {
+        if (Common::getEventString(i).compare("EV_UNKNOWN") == 0)
+        {
+            break;
+        }
+        cout << Common::getEventString(i) << "," << i << endl;
+    }
+    return 0;
+}
diff --git a/src/scripts/EventDumper/Makefile b/src/scripts/EventDumper/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..61729aa2876d8cb8188f38b05fe68bbce4dc065e
--- /dev/null
+++ b/src/scripts/EventDumper/Makefile
@@ -0,0 +1,16 @@
+BOARDCORE := ../../../skyward-boardcore/
+OBSW := ../../../src/boards/
+
+all:
+	g++ -std=c++17 -o eventdumper EventDumper.cpp \
+					-DCOMPILE_FOR_X86 \
+					-DCOMPILE_FOR_HOST \
+					$(BOARDCORE)libs/tscpp/tscpp/stream.cpp \
+					-I$(BOARDCORE)libs/miosix-host \
+	 				-I$(BOARDCORE)libs/mavlink-skyward-lib \
+	 				-I$(BOARDCORE)libs/eigen \
+	 				-I$(BOARDCORE)libs/tscpp \
+	 				-I$(BOARDCORE)src/shared \
+					-I$(OBSW)
+clean:
+	rm logdecoder
diff --git a/src/scripts/eventgen.sh b/src/scripts/eventgen.sh
new file mode 100755
index 0000000000000000000000000000000000000000..6763addd5072948bc8e191ff4e61e4c31c20494a
--- /dev/null
+++ b/src/scripts/eventgen.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Authors: Alberto Nidasio
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/generators/eventgen.py $@
diff --git a/src/scripts/fsmgen.sh b/src/scripts/fsmgen.sh
new file mode 100755
index 0000000000000000000000000000000000000000..2bae83d7f938ec78624784bfbb93aaa0f17e4431
--- /dev/null
+++ b/src/scripts/fsmgen.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Authors: Alberto Nidasio
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/generators/fsmgen.py $@
diff --git a/src/scripts/logdecoder/Main/Makefile b/src/scripts/logdecoder/Main/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..01b0967730c29d85054d1eb3e3df4b1cc605d255
--- /dev/null
+++ b/src/scripts/logdecoder/Main/Makefile
@@ -0,0 +1,18 @@
+BOARDCORE := ../../../../skyward-boardcore/
+OBSW := ../../../../src/boards/
+HIL := ../../../../src/
+
+all:
+	g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \
+					-DCOMPILE_FOR_X86 \
+					-DCOMPILE_FOR_HOST \
+					$(BOARDCORE)libs/tscpp/tscpp/stream.cpp \
+					-I$(BOARDCORE)libs/miosix-host \
+	 				-I$(BOARDCORE)libs/mavlink-skyward-lib \
+	 				-I$(BOARDCORE)libs/eigen \
+	 				-I$(BOARDCORE)libs/tscpp \
+	 				-I$(BOARDCORE)src/shared \
+					-I$(OBSW) \
+					-I$(HIL)
+clean:
+	rm logdecoder
diff --git a/src/scripts/logdecoder/Main/logdecoder.cpp b/src/scripts/logdecoder/Main/logdecoder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e656031c7fe16b3ea8d98bb2fc15c57f4cbb040f
--- /dev/null
+++ b/src/scripts/logdecoder/Main/logdecoder.cpp
@@ -0,0 +1,158 @@
+/* Copyright (c) 2018-2022 Skyward Experimental Rocketry
+ * Author: 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 <Main/Sensors/SensorsData.h>
+#include <Main/CanHandler/CanHandlerData.h>
+#include <Main/PinHandler/PinData.h>
+#include <Main/Sensors/RotatedIMU/RotatedIMUData.h>
+#include <Main/Sensors/SensorsData.h>
+#include <Main/StateMachines/ABKController/ABKControllerData.h>
+#include <Main/StateMachines/ADAController/ADAControllerData.h>
+#include <Main/StateMachines/Deployment/DeploymentData.h>
+#include <Main/StateMachines/FlightModeManager/FlightModeManagerData.h>
+#include <Main/StateMachines/MEAController/MEAControllerData.h>
+#include <Main/StateMachines/NASController/NASControllerData.h>
+#include <algorithms/MEA/MEAData.h>
+#include <hardware_in_the_loop/HIL_sensors/HILSensorsData.h>
+#include <logger/Deserializer.h>
+#include <logger/LogTypes.h>
+#include <tscpp/stream.h>
+
+#include <fstream>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+
+/**
+ * @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 tscpp;
+using namespace Boardcore;
+using namespace Main;
+
+void registerTypes(Deserializer& ds)
+{
+    // Register all Boardcore types
+    LogTypes::registerTypes(ds);
+
+    // Custom types
+    ds.registerType<FlightModeManagerStatus>();
+    ds.registerType<NASControllerStatus>();
+    ds.registerType<MEAControllerStatus>();
+    ds.registerType<MEAState>();
+    ds.registerType<ADAControllerStatus>();
+    ds.registerType<ABKControllerStatus>();
+    ds.registerType<DeploymentStatus>();
+    ds.registerType<LPS28DFW_1Data>();
+    ds.registerType<LPS28DFW_2Data>();
+    ds.registerType<HSCMRNN015PA_1Data>();
+    ds.registerType<HSCMRNN015PA_2Data>();
+    ds.registerType<RotatedIMUData>();
+    ds.registerType<CanPressureSensor>();
+    ds.registerType<CanTemperatureSensor>();
+    ds.registerType<CanCurrentSensor>();
+    ds.registerType<CanVoltageSensor>();
+    ds.registerType<SensorsCalibrationParameter>();
+    ds.registerType<PinChangeData>();
+    ds.registerType<HILAccelerometerData>();
+    ds.registerType<HILBarometerData>();
+    ds.registerType<HILGpsData>();
+    ds.registerType<HILGyroscopeData>();
+    ds.registerType<HILImuData>();
+    ds.registerType<HILMagnetometerData>();
+    ds.registerType<HILPitotData>();
+    ds.registerType<HILTempData>();
+}
+
+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);
+    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);
+
+    // End
+    if (success)
+        std::cout << "Deserialization completed successfully\n";
+    else
+        std::cout << "Deserialization ended with errors\n";
+    return 0;
+}