diff --git a/CMakeLists.txt b/CMakeLists.txt
index 14369b6886f36b9b733b8f5cce41cce13e871c30..e50a959a0d54b57a185d1331bb1f1b49add26e68 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,6 +56,11 @@ add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES
 target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS})
 sbs_target(motor-entry stm32f767zi_gemini_motor)
 
+add_executable(rig-entry src/entrypoints/RIG/rig-entry.cpp ${RIG_COMPUTER})
+target_include_directories(rig-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(rig-entry stm32f429zi_skyward_rig)
+
 #-----------------------------------------------------------------------------#
 #                               Test entrypoints                              #
-#-----------------------------------------------------------------------------#
\ No newline at end of file
+#-----------------------------------------------------------------------------#
+
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 098b277a6b2fc13b067e467dbf74fcd6c571a31d..65ae83089d526ae7967dc9be87e3a8175b094772 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -54,4 +54,16 @@ set(MOTOR_SOURCES
     src/boards/Motor/Sensors/Sensors.cpp
     src/boards/Motor/BoardScheduler.cpp
     src/boards/Motor/CanHandler/CanHandler.cpp
+) 
+
+set(RIG_COMPUTER
+    src/boards/RIG/BoardScheduler.cpp
+    src/boards/RIG/Sensors/Sensors.cpp
+    src/boards/RIG/Actuators/Actuators.cpp
+    src/boards/RIG/Radio/Radio.cpp
+    src/boards/RIG/TMRepository/TMRepository.cpp
+    src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp
+    src/boards/RIG/StateMachines/TARS1/TARS1.cpp
+    src/boards/RIG/CanHandler/CanHandler.cpp
+    src/boards/RIG/StatesMonitor/StatesMonitor.cpp
 )
diff --git a/src/boards/RIG/Actuators/Actuators.cpp b/src/boards/RIG/Actuators/Actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0273849a869ffe503fa76eda11a5d61b6ac51adf
--- /dev/null
+++ b/src/boards/RIG/Actuators/Actuators.cpp
@@ -0,0 +1,413 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/Actuators/ActuatorsData.h>
+#include <RIG/CanHandler/CanHandler.h>
+#include <RIG/Configs/ActuatorsConfig.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+namespace RIG
+{
+Actuators::Actuators(TaskScheduler* sched) : scheduler(sched)
+{
+    servo1 =
+        new Servo(Config::Servos::SERVO1_TIMER, Config::Servos::SERVO1_PWM_CH,
+                  Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE);
+    servo2 =
+        new Servo(Config::Servos::SERVO2_TIMER, Config::Servos::SERVO2_PWM_CH,
+                  Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    servo3 =
+        new Servo(Config::Servos::SERVO3_TIMER, Config::Servos::SERVO3_PWM_CH,
+                  Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE);
+    servo4 =
+        new Servo(Config::Servos::SERVO4_TIMER, Config::Servos::SERVO4_PWM_CH,
+                  Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+    servo5 =
+        new Servo(Config::Servos::SERVO5_TIMER, Config::Servos::SERVO5_PWM_CH,
+                  Config::Servos::MAX_PULSE, Config::Servos::MIN_PULSE);
+
+    // Set the default openings
+    openings[ServosList::FILLING_VALVE] =
+        Config::Servos::DEFAULT_FILLING_MAXIMUM_APERTURE;
+    openings[ServosList::MAIN_VALVE] =
+        Config::Servos::DEFAULT_MAIN_MAXIMUM_APERTURE;
+    openings[ServosList::RELEASE_VALVE] =
+        Config::Servos::DEFAULT_RELEASE_MAXIMUM_APERTURE;
+    openings[ServosList::VENTING_VALVE] =
+        Config::Servos::DEFAULT_VENTING_MAXIMUM_APERTURE;
+    openings[ServosList::DISCONNECT_SERVO] =
+        Config::Servos::DEFAULT_DISCONNECT_MAXIMUM_APERTURE;
+
+    // Set the default opening times
+    openingTimes[ServosList::FILLING_VALVE] =
+        Config::Servos::DEFAULT_FILLING_OPENING_TIME;
+    openingTimes[ServosList::MAIN_VALVE] =
+        Config::Servos::DEFAULT_MAIN_OPENING_TIME;
+    openingTimes[ServosList::RELEASE_VALVE] =
+        Config::Servos::DEFAULT_RELEASE_OPENING_TIME;
+    openingTimes[ServosList::VENTING_VALVE] =
+        Config::Servos::DEFAULT_VENTING_OPENING_TIME;
+    openingTimes[ServosList::DISCONNECT_SERVO] =
+        Config::Servos::DEFAULT_DISCONNECT_OPENING_TIME;
+
+    // Set the opening / closing events
+    openingEvents[ServosList::FILLING_VALVE] =
+        Common::Events::MOTOR_OPEN_FILLING_VALVE;
+    openingEvents[ServosList::MAIN_VALVE] =
+        Common::Events::MOTOR_OPEN_FEED_VALVE;
+    openingEvents[ServosList::RELEASE_VALVE] =
+        Common::Events::MOTOR_OPEN_RELEASE_VALVE;
+    openingEvents[ServosList::VENTING_VALVE] =
+        Common::Events::MOTOR_OPEN_VENTING_VALVE;
+    openingEvents[ServosList::DISCONNECT_SERVO] =
+        Common::Events::MOTOR_DISCONNECT;
+
+    closingEvents[ServosList::FILLING_VALVE] =
+        Common::Events::MOTOR_CLOSE_FILLING_VALVE;
+    closingEvents[ServosList::MAIN_VALVE] =
+        Common::Events::MOTOR_CLOSE_FEED_VALVE;
+    closingEvents[ServosList::RELEASE_VALVE] =
+        Common::Events::MOTOR_CLOSE_RELEASE_VALVE;
+    closingEvents[ServosList::VENTING_VALVE] =
+        Common::Events::MOTOR_CLOSE_VENTING_VALVE;
+}
+
+bool Actuators::start()
+{
+    servo1->enable();
+    servo2->enable();
+    servo3->enable();
+    servo4->enable();
+    servo5->enable();
+
+    uint8_t result = scheduler->addTask(
+        [=]() { checkTimings(); }, Config::Servos::SERVO_TIMINGS_CHECK_PERIOD);
+    return result != 0;
+}
+
+bool Actuators::wiggleServo(ServosList servo)
+{
+    PauseKernelLock lock;
+    // Get the choosen servo and nullptr in case not present
+    Servo* requestedServo = getServo(servo);
+    if (requestedServo != nullptr)
+    {
+        // Close all the servo
+        closeAllServo();
+
+        // Store the previous aperture time
+        uint64_t time = openingTimes[servo];
+
+        // Set a new one
+        openingTimes[servo] = 1000;  // 1s
+
+        // Toggle the servo
+        toggleServo(servo);
+
+        // Set the previous opening time
+        openingTimes[servo] = time;
+
+        return true;
+    }
+
+    return false;
+}
+
+Servo* Actuators::getServo(ServosList servo)
+{
+    switch (servo)
+    {
+        case MAIN_VALVE:
+            return servo4;
+        case VENTING_VALVE:
+            return servo5;
+        case RELEASE_VALVE:
+            return servo2;
+        case FILLING_VALVE:
+            return servo1;
+        case DISCONNECT_SERVO:
+            return servo3;
+        default:
+            return nullptr;
+    }
+}
+
+float Actuators::getServoPosition(ServosList servo)
+{
+    // Get the choosen servo and nullptr in case not present
+    Servo* requestedServo = getServo(servo);
+
+    if (requestedServo == nullptr)
+    {
+        return -1;
+    }
+
+    return requestedServo->getPosition();
+}
+
+void Actuators::setServoPosition(ServosList servo, float position)
+{
+    // To lock the resources use a kernel lock
+    PauseKernelLock lock;
+
+    // Get the choosen servo and nullptr in case not present
+    Servo* requestedServo = getServo(servo);
+
+    if (!(requestedServo == nullptr))
+    {
+        requestedServo->setPosition(position);
+
+        // Log the position
+        ActuatorsData data;
+        data.timestamp = TimestampTimer::getTimestamp();
+        data.servoId   = servo;
+        data.position  = position;
+        Logger::getInstance().log(data);
+    }
+}
+
+void Actuators::checkTimings()
+{
+    uint64_t currentTick = getTick();
+
+    // Enter in protected zone where the timings should be checked and changed
+    // and the servo should be positioned atomically over all the threads. A
+    // kernel lock is adopted only due to it's performance but a mutex could be
+    // easily adopted.
+    {
+        PauseKernelLock lock;
+
+        for (uint8_t i = 0; i < ServosList::ServosList_ENUM_END; i++)
+        {
+            if (timings[i] > currentTick)
+            {
+                if (currentTick >
+                    setFlag[i] + Config::Servos::SERVO_CONFIDENCE_TIME)
+                {
+                    setServoPosition(
+                        static_cast<ServosList>(i),
+                        openings[i] -
+                            openings[i] *
+                                Config::Servos::
+                                    SERVO_CONFIDENCE);  // 2% less than the
+                                                        // actual aperture
+                }
+                else
+                {
+                    // Open the corresponding valve wrt the maximum opening
+                    setServoPosition(static_cast<ServosList>(i), openings[i]);
+                }
+            }
+            else
+            {
+                if (timings[i] != 0)
+                {
+                    timings[i] = 0;
+                    setFlag[i] = currentTick;
+
+                    {
+                        RestartKernelLock l(lock);
+
+                        // Publish the closing event
+                        EventBroker::getInstance().post(
+                            closingEvents[i], Common::Topics::TOPIC_MOTOR);
+                    }
+                }
+                if (currentTick >
+                    setFlag[i] + Config::Servos::SERVO_CONFIDENCE_TIME)
+                {
+                    setServoPosition(
+                        static_cast<ServosList>(i),
+                        openings[i] *
+                            Config::Servos::SERVO_CONFIDENCE);  // 2% open
+                }
+                else
+                {
+                    setServoPosition(static_cast<ServosList>(i), 0);
+                }
+            }
+        }
+    }
+}
+
+void Actuators::toggleServo(ServosList servo)
+{
+    PauseKernelLock lock;
+
+    if (getServo(servo) != nullptr)
+    {
+        // If the valve is already open
+        if (timings[servo] > 0)
+        {
+            timings[servo] = 0;
+            setFlag[servo] = getTick();
+
+            {
+                RestartKernelLock l(lock);
+
+                // Publish the closing event
+                EventBroker::getInstance().post(closingEvents[servo],
+                                                Common::Topics::TOPIC_MOTOR);
+
+                // Publish the command also into the CAN bus
+                ModuleManager::getInstance()
+                    .get<CanHandler>()
+                    ->sendCanServoCommand(servo, 0, 0);
+            }
+        }
+        else
+        {
+            timings[servo] = getTick() + openingTimes[servo];
+            setFlag[servo] = getTick();
+
+            {
+                RestartKernelLock l(lock);
+
+                // Publish the opening event
+                EventBroker::getInstance().post(openingEvents[servo],
+                                                Common::Topics::TOPIC_MOTOR);
+
+                // Publish the command also into the CAN bus
+                ModuleManager::getInstance()
+                    .get<CanHandler>()
+                    ->sendCanServoCommand(servo, 1, openingTimes[servo]);
+            }
+        }
+    }
+}
+
+void Actuators::openServoAtomic(ServosList servo, uint32_t time)
+{
+    PauseKernelLock lock;
+
+    if (getServo(servo) != nullptr)
+    {
+        // If the valve is already open
+        if (timings[servo] > 0)
+        {
+            timings[servo] = 0;
+            setFlag[servo] = getTick();
+
+            {
+                RestartKernelLock l(lock);
+
+                // Publish the closing event
+                EventBroker::getInstance().post(closingEvents[servo],
+                                                Common::Topics::TOPIC_MOTOR);
+
+                // Publish the command also into the CAN bus
+                ModuleManager::getInstance()
+                    .get<CanHandler>()
+                    ->sendCanServoCommand(servo, 0, 0);
+            }
+        }
+        else
+        {
+            timings[servo] = getTick() + time;
+            setFlag[servo] = getTick();
+
+            {
+                RestartKernelLock l(lock);
+
+                // Publish the opening event
+                EventBroker::getInstance().post(openingEvents[servo],
+                                                Common::Topics::TOPIC_MOTOR);
+
+                // Publish the command also into the CAN bus
+                ModuleManager::getInstance()
+                    .get<CanHandler>()
+                    ->sendCanServoCommand(servo, 1, time);
+            }
+        }
+    }
+}
+
+void Actuators::closeAllServo()
+{
+    PauseKernelLock lock;
+
+    for (uint8_t i = 0; i < ServosList::ServosList_ENUM_END; i++)
+    {
+        // Once the disconnect servo is open it shall never close
+        if (timings[i] != 0 && i != ServosList::DISCONNECT_SERVO)
+        {
+            // Make the timings expire
+            timings[i] = 0;
+            setFlag[i] = getTick();
+
+            // Publish the command also into the CAN bus
+            ModuleManager::getInstance().get<CanHandler>()->sendCanServoCommand(
+                static_cast<ServosList>(i), 0, 0);
+        }
+    }
+}
+
+void Actuators::setMaximumAperture(ServosList servo, float aperture)
+{
+    PauseKernelLock lock;
+
+    // Check if the servo exists
+    if (getServo(servo) != nullptr)
+    {
+        // Check aperture
+        if (aperture < 0)
+        {
+            aperture = 0;
+        }
+        else if (aperture > 1)
+        {
+            aperture = 1;
+        }
+
+        openings[servo] = aperture;
+    }
+}
+
+void Actuators::setTiming(ServosList servo, uint32_t time)
+{
+    PauseKernelLock lock;
+
+    // Check if the servo exists
+    if (getServo(servo) != nullptr)
+    {
+        openingTimes[servo] = time;
+    }
+}
+
+uint32_t Actuators::getTiming(ServosList servo)
+{
+    PauseKernelLock lock;
+
+    if (getServo(servo) != nullptr)
+    {
+        return openingTimes[servo];
+    }
+    return 0;
+}
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Actuators/Actuators.h b/src/boards/RIG/Actuators/Actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..b6e2118eedaf08af26518087343f709920934948
--- /dev/null
+++ b/src/boards/RIG/Actuators/Actuators.h
@@ -0,0 +1,158 @@
+/* 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.
+ */
+#pragma once
+
+#include <actuators/Servo/Servo.h>
+#include <common/Events.h>
+#include <common/Mavlink.h>
+#include <events/EventBroker.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+class Actuators : public Boardcore::Module
+{
+public:
+    /**
+     * @brief Construct a new Actuators object
+     *
+     * @param sched The scheduler to respect the atomic timings in case of TARS0
+     * engaged
+     */
+    Actuators(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Enables all the servos PWMs and adds to the scheduler a periodic
+     * method to check whether the single servos time expired
+     */
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Wiggles the requested servo for 1 second
+     *
+     * @param servo The Mavlink requested servo
+     * @returns False if the servo does not appear in the list
+     */
+    bool wiggleServo(ServosList servo);
+
+    /**
+     * @brief Get the specified Servo's Position
+     *
+     * @param servo The Mavlink requested servo
+     * @return float The servo position in normalized notation [0-1]
+     */
+    float getServoPosition(ServosList servo);
+
+    /**
+     * @brief Toggles the servo valve passed via parameter. It sets the timings
+     * such that the valve opens for a certain amount of time, otherwise, if the
+     * valve is already open it closes it.
+     *
+     * @param servo The servo valve to toggle
+     */
+    void toggleServo(ServosList servo);
+
+    /**
+     * @brief Opens the servo valve passed via parameter for a certain amount of
+     * time. If the valve is already open it closes it.
+     *
+     * @param servo The servo valve to open
+     * @param time The time in [ms]
+     */
+    void openServoAtomic(ServosList servo, uint32_t time);
+
+    /**
+     * @brief Closes all the servo valves
+     */
+    void closeAllServo();
+
+    /**
+     * @brief Sets a servo maximum aperture in normalized form. [0,1]
+     *
+     * @param servo The servo to which set the aperture
+     * @param aperture The aperture in [0,1]
+     */
+    void setMaximumAperture(ServosList servo, float aperture);
+
+    /**
+     * @brief Set the timing of a certain valve.
+     * @note THE TIMING IS IN [ms]
+     *
+     * @param servo The servo to which set the timing
+     * @param time The timing in [ms]
+     */
+    void setTiming(ServosList servo, uint32_t time);
+
+    /**
+     * @brief Get the Timing of the passed servo
+     *
+     * @param servo The servo whose timing is needed
+     * @return uint32_t The timing in [ms]
+     */
+    uint32_t getTiming(ServosList servo);
+
+private:
+    /**
+     * @brief Set the Servo's position to the parameter one
+     *
+     * @param servo The servo to move
+     * @param position A normalized position [0, 1]
+     */
+    void setServoPosition(ServosList servo, float position);
+
+    Boardcore::Servo* getServo(ServosList servo);
+
+    /**
+     * @brief Checks that the atomic timings for the servos didn't expire and
+     * sets the positioning according to them.
+     * @note Utilizes also the variable setFlag to understand when the position
+     * was set the last time. If greater than CONSTANT seconds ago, the function
+     * sets a little offset to the servo to avoid power angriness.
+     */
+    void checkTimings();
+
+    // Create the list of timings for every servo
+    uint64_t timings[ServosList::ServosList_ENUM_END] = {0};
+
+    // This set of flags helps the controller to know when the servo have been
+    // set, in order to change slightly their angle after CONSTANT time, to
+    // avoid over consumption
+    uint64_t setFlag[ServosList::ServosList_ENUM_END]      = {0};
+    float openings[ServosList::ServosList_ENUM_END]        = {1};
+    uint64_t openingTimes[ServosList::ServosList_ENUM_END] = {
+        100000};  // Default 100s
+
+    // This set represents the events to throw at opening/closing of valves
+    uint8_t openingEvents[ServosList::ServosList_ENUM_END] = {0};
+    uint8_t closingEvents[ServosList::ServosList_ENUM_END] = {0};
+
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    Boardcore::Servo* servo1;
+    Boardcore::Servo* servo2;
+    Boardcore::Servo* servo3;
+    Boardcore::Servo* servo4;
+    Boardcore::Servo* servo5;
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Actuators/ActuatorsData.h b/src/boards/RIG/Actuators/ActuatorsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..a233b16336e9356cecdfca9660462869414b268c
--- /dev/null
+++ b/src/boards/RIG/Actuators/ActuatorsData.h
@@ -0,0 +1,57 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <ostream>
+
+namespace RIG
+{
+struct ActuatorsData
+{
+    uint64_t timestamp;
+    uint8_t servoId;
+    float position;
+
+    ActuatorsData()
+    {
+        timestamp = 0;
+        servoId   = 0;
+        position  = 0;
+    }
+
+    ActuatorsData(uint64_t time, uint8_t servo, float pos)
+        : timestamp(time), servoId(servo), position(pos)
+    {
+    }
+
+    static std::string header() { return "timestamp,servoId,position\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)servoId << "," << position << "\n";
+    }
+};
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/BoardScheduler.cpp b/src/boards/RIG/BoardScheduler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f8eff0cd7d8a403db8603b003260f917d71ff91
--- /dev/null
+++ b/src/boards/RIG/BoardScheduler.cpp
@@ -0,0 +1,65 @@
+/* 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 "BoardScheduler.h"
+
+using namespace Boardcore;
+
+namespace RIG
+{
+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 RIG
\ No newline at end of file
diff --git a/src/boards/RIG/BoardScheduler.h b/src/boards/RIG/BoardScheduler.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2d878112be17a233b7c6a1ab83bf255f95b5591
--- /dev/null
+++ b/src/boards/RIG/BoardScheduler.h
@@ -0,0 +1,62 @@
+/* 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.
+ */
+#pragma once
+
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+/**
+ * @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 RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Buses.h b/src/boards/RIG/Buses.h
new file mode 100644
index 0000000000000000000000000000000000000000..27a4fd08616256dc28ebfad401f39cd44d365b52
--- /dev/null
+++ b/src/boards/RIG/Buses.h
@@ -0,0 +1,45 @@
+/* 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.
+ */
+#pragma once
+
+#include <drivers/spi/SPIBus.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+class Buses : public Boardcore::Module
+{
+public:
+    Boardcore::SPIBus spi1;
+    Boardcore::SPIBus spi2;
+    Boardcore::SPIBus spi3;
+    Boardcore::SPIBus spi4;
+    Boardcore::SPIBus spi5;
+    Boardcore::SPIBus spi6;
+
+    Buses()
+        : spi1(SPI1), spi2(SPI2), spi3(SPI3), spi4(SPI4), spi5(SPI5), spi6(SPI6)
+    {
+    }
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/CanHandler/CanHandler.cpp b/src/boards/RIG/CanHandler/CanHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..913bbe3016fadeadcdff32227022107c46833532
--- /dev/null
+++ b/src/boards/RIG/CanHandler/CanHandler.cpp
@@ -0,0 +1,314 @@
+/* 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 <RIG/BoardScheduler.h>
+#include <RIG/Buses.h>
+#include <RIG/CanHandler/CanHandlerData.h>
+#include <RIG/Configs/CanHandlerConfig.h>
+#include <RIG/Sensors/Sensors.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIG/StatesMonitor/StatesMonitor.h>
+#include <common/CanConfig.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.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 RIG::CanHandlerConfig;
+
+namespace RIG
+{
+CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched)
+{
+    // Configure the CAN driver
+    CanbusDriver::AutoBitTiming bitTiming;
+    bitTiming.baudRate    = BAUD_RATE;
+    bitTiming.samplePoint = SAMPLE_POINT;
+
+    CanbusDriver::CanbusConfig config;
+
+    // NOTE configure the peripheral CAN1 due to shared configs
+    driver = new CanbusDriver(CAN1, config, bitTiming);
+
+    // Create the protocol with the defined driver
+    protocol =
+        new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1),
+                        miosix::PRIORITY_MAX - 1);
+
+    // Accept messages only from the main and RIG board
+    protocol->addFilter(static_cast<uint8_t>(Board::MAIN),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::MOTOR),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    protocol->addFilter(static_cast<uint8_t>(Board::PAYLOAD),
+                        static_cast<uint8_t>(Board::BROADCAST));
+    driver->init();
+}
+
+bool CanHandler::start()
+{
+    // 0 if fail
+    uint8_t result = scheduler->addTask(  // status
+        [&]()
+        {
+            GroundModeManagerState state = ModuleManager::getInstance()
+                                               .get<GroundModeManager>()
+                                               ->getStatus()
+                                               .state;
+            protocol->enqueueSimplePacket(
+                static_cast<uint8_t>(Priority::MEDIUM),
+                static_cast<uint8_t>(PrimaryType::STATUS),
+                static_cast<uint8_t>(Board::RIG),
+                static_cast<uint8_t>(Board::BROADCAST),
+                static_cast<uint8_t>(state),
+                (state == GroundModeManagerState::ARMED) ? 0x01 : 0x00);
+        },
+        STATUS_TRANSMISSION_PERIOD);
+    // TODO look at the priorities of the CAN protocol threads
+    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::RIG),
+                           static_cast<uint8_t>(Board::BROADCAST),
+                           static_cast<uint8_t>(event));
+}
+
+void CanHandler::sendCanServoCommand(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::RIG),
+                                  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::STATUS:
+        {
+            handleCanStatus(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::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_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;
+        }
+        case SensorId::MAIN_BOARD_CURRENT:
+        {
+            CurrentData data = currentDataFromCanMessage(msg);
+            modules.get<Sensors>()->setMainCurrent(data);
+
+            // Log the data
+            CanCurrentSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.sensorId  = static_cast<uint8_t>(SensorId::MAIN_BOARD_CURRENT);
+            log.current   = data.current;
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::PAYLOAD_BOARD_CURRENT:
+        {
+            CurrentData data = currentDataFromCanMessage(msg);
+            modules.get<Sensors>()->setPayloadCurrent(data);
+
+            // Log the data
+            CanCurrentSensor log;
+            log.timestamp = TimestampTimer::getTimestamp();
+            log.sensorId =
+                static_cast<uint8_t>(SensorId::PAYLOAD_BOARD_CURRENT);
+            log.current = data.current;
+            Logger::getInstance().log(log);
+
+            break;
+        }
+        case SensorId::MOTOR_BOARD_VOLTAGE:
+        {
+            BatteryVoltageSensorData data = voltageDataFromCanMessage(msg);
+            modules.get<Sensors>()->setMotorVoltage(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;
+        }
+        default:
+        {
+            LOG_WARN(logger, "Received unsupported sensor data: id={}",
+                     sensorId);
+        }
+    }
+}
+
+void CanHandler::handleCanStatus(const CanMessage &msg)
+{
+    Board source = static_cast<Board>(msg.getSource());
+    // uint8_t status = msg.getSecondaryType();
+    bool armed = msg.payload[0];
+
+    // Set the state inside the monitor
+    ModuleManager::getInstance().get<StatesMonitor>()->setBoardStatus(
+        source, armed ? 2 : 1);
+}
+
+}  // namespace RIG
diff --git a/src/boards/RIG/CanHandler/CanHandler.h b/src/boards/RIG/CanHandler/CanHandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..071a9f45c9bf79a0f2d1a575686b786f70a7359f
--- /dev/null
+++ b/src/boards/RIG/CanHandler/CanHandler.h
@@ -0,0 +1,86 @@
+/* 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 <RIG/BoardScheduler.h>
+#include <common/CanConfig.h>
+#include <common/Mavlink.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+
+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 sendCanServoCommand(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 handleCanStatus(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 RIG
diff --git a/src/boards/RIG/CanHandler/CanHandlerData.h b/src/boards/RIG/CanHandler/CanHandlerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..be4ab08bda9c6216467762ef3d8524ee9f57a78d
--- /dev/null
+++ b/src/boards/RIG/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 RIG
+{
+// 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 RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/ActuatorsConfig.h b/src/boards/RIG/Configs/ActuatorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..78c7081d9f3a6f9afb8c063517130bc7f0c44460
--- /dev/null
+++ b/src/boards/RIG/Configs/ActuatorsConfig.h
@@ -0,0 +1,69 @@
+/* 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.
+ */
+#pragma once
+#include <drivers/timer/PWM.h>
+#include <drivers/timer/TimerUtils.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace Servos
+{
+static TIM_TypeDef* const SERVO1_TIMER = TIM4;
+static TIM_TypeDef* const SERVO2_TIMER = TIM11;
+static TIM_TypeDef* const SERVO3_TIMER = TIM3;
+static TIM_TypeDef* const SERVO4_TIMER = TIM10;
+static TIM_TypeDef* const SERVO5_TIMER = TIM8;
+
+constexpr Boardcore::TimerUtils::Channel SERVO1_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_2;
+constexpr Boardcore::TimerUtils::Channel SERVO2_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+constexpr Boardcore::TimerUtils::Channel SERVO3_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+constexpr Boardcore::TimerUtils::Channel SERVO4_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+constexpr Boardcore::TimerUtils::Channel SERVO5_PWM_CH =
+    Boardcore::TimerUtils::Channel::CHANNEL_1;
+
+constexpr uint16_t MIN_PULSE = 900;
+constexpr uint16_t MAX_PULSE = 2000;
+
+constexpr uint16_t SERVO_TIMINGS_CHECK_PERIOD = 100;
+constexpr uint16_t SERVO_CONFIDENCE_TIME      = 500;       // 0.5s
+constexpr float SERVO_CONFIDENCE              = 1 / 50.0;  // 2%
+
+constexpr uint32_t DEFAULT_FILLING_OPENING_TIME    = 15000;  // 15s
+constexpr uint32_t DEFAULT_VENTING_OPENING_TIME    = 15000;  // 15s
+constexpr uint32_t DEFAULT_MAIN_OPENING_TIME       = 6000;   // 6s
+constexpr uint32_t DEFAULT_RELEASE_OPENING_TIME    = 10000;  // 10s
+constexpr uint32_t DEFAULT_DISCONNECT_OPENING_TIME = 10000;  // 10s
+
+constexpr float DEFAULT_FILLING_MAXIMUM_APERTURE    = 0.97f;
+constexpr float DEFAULT_VENTING_MAXIMUM_APERTURE    = 0.40f;
+constexpr float DEFAULT_MAIN_MAXIMUM_APERTURE       = 0.87f;
+constexpr float DEFAULT_RELEASE_MAXIMUM_APERTURE    = 0.97f;
+constexpr float DEFAULT_DISCONNECT_MAXIMUM_APERTURE = 0.110f;
+}  // namespace Servos
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/CanHandlerConfig.h b/src/boards/RIG/Configs/CanHandlerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf32a6a5f94142f082b48348b2263803c359ee7e
--- /dev/null
+++ b/src/boards/RIG/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 RIG
+{
+
+namespace CanHandlerConfig
+{
+
+constexpr unsigned int PITOT_TRANSMISSION_PERIOD  = 20;    // ms
+constexpr unsigned int STATUS_TRANSMISSION_PERIOD = 2000;  // ms
+
+}  // namespace CanHandlerConfig
+
+}  // namespace RIG
diff --git a/src/boards/RIG/Configs/GroundModeManagerConfig.h b/src/boards/RIG/Configs/GroundModeManagerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7e69b7a3c568b9c0904db3e9974052666848983
--- /dev/null
+++ b/src/boards/RIG/Configs/GroundModeManagerConfig.h
@@ -0,0 +1,40 @@
+/* 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 RIG
+{
+namespace Config
+{
+namespace Ignition
+{
+constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 5451;  // ms
+
+constexpr uint32_t DEAD_TIME_AFTER_BURN  = 900;  // ms
+constexpr uint32_t NUMBER_NITROGEN_BUMPS = 15;
+constexpr uint32_t NITROGEN_OPENING_TIME = 2700;  // ms
+
+}  // namespace Ignition
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/RadioConfig.h b/src/boards/RIG/Configs/RadioConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..b84d67c11b3d4f3f1f1ae811ab9f3dd2a7947e6f
--- /dev/null
+++ b/src/boards/RIG/Configs/RadioConfig.h
@@ -0,0 +1,56 @@
+/* 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.
+ */
+#pragma once
+
+#include <common/Mavlink.h>
+#include <stdint.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace Radio
+{
+// 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;
+
+// Mavlink driver parameters
+constexpr uint16_t MAV_SLEEP_AFTER_SEND   = 0;
+constexpr uint16_t MAV_OUT_BUFFER_MAX_AGE = 10;
+
+// Mavlink ids
+constexpr uint8_t MAV_SYSTEM_ID    = 171;
+constexpr uint8_t MAV_COMPONENT_ID = 96;
+
+// Circular buffer size (number of maximum messages that the RIG can answer
+// after a ping message from conRIG)
+constexpr uint8_t RADIO_CIRCULAR_BUFFER_SIZE = 6;
+
+// Threshold to consider a new command correct and not a false trigger.
+// Expressed in [ms]
+constexpr long long int RADIO_LAST_COMMAND_THRESHOLD = 1000;
+
+}  // namespace Radio
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/RefuelingConfig.h b/src/boards/RIG/Configs/RefuelingConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ab25b2ae9cac31a41a2df42e6a3ef8fa9f30c0f
--- /dev/null
+++ b/src/boards/RIG/Configs/RefuelingConfig.h
@@ -0,0 +1,35 @@
+/* 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.
+ */
+#pragma once
+
+#include <stdint.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace Refueling
+{
+
+}  // namespace Refueling
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/SensorsConfig.h b/src/boards/RIG/Configs/SensorsConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..1747a340907bdcfca9a46d30bd8e3ac9183f9aa4
--- /dev/null
+++ b/src/boards/RIG/Configs/SensorsConfig.h
@@ -0,0 +1,61 @@
+/* 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.
+ */
+#pragma once
+#include <stdint.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace Sensors
+{
+constexpr uint16_t ADC_SAMPLE_PERIOD          = 10;
+constexpr uint16_t LOAD_CELL_SAMPLE_PERIOD    = 13;
+constexpr uint16_t THERMOCOUPLE_SAMPLE_PERIOD = 100;
+
+constexpr float ADC1_CH1_SHUNT_RESISTANCE = 7.3;
+constexpr float ADC1_CH2_SHUNT_RESISTANCE = 7.3;
+constexpr float ADC1_CH3_SHUNT_RESISTANCE = 7.3;
+constexpr float ADC1_CH4_SHUNT_RESISTANCE = 7.3;
+
+constexpr float ADC2_CH1_SHUNT_RESISTANCE = 7.3;
+
+constexpr float FILLING_MAX_PRESSURE     = 100;  // bar
+constexpr float TANK_TOP_MAX_PRESSURE    = 100;  // bar
+constexpr float TANK_BOTTOM_MAX_PRESSURE = 100;  // bar
+constexpr float VESSEL_MAX_PRESSURE      = 400;  // bar
+
+constexpr float SENSOR_MIN_CURRENT = 4;   // mA
+constexpr float SENSOR_MAX_CURRENT = 20;  // mA
+
+constexpr float LOAD_CELL2_OFFSET = -38150;
+constexpr float LOAD_CELL1_OFFSET = -93539.8f;
+
+constexpr float LOAD_CELL2_SCALE = -5.6188e-5f;
+constexpr float LOAD_CELL1_SCALE = -2.251307e-5f;
+
+constexpr float VOLTAGE_CONVERSION_FACTOR         = 13.58f;
+constexpr float CURRENT_CONVERSION_FACTOR         = 13.645f;
+constexpr float VOLTAGE_CURRENT_CONVERSION_FACTOR = 0.020f;  // 40 mV/A
+}  // namespace Sensors
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/StatesMonitorConfig.h b/src/boards/RIG/Configs/StatesMonitorConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..37d3eeefec522bb5c1c2e3f61b9bd2b489d7a78f
--- /dev/null
+++ b/src/boards/RIG/Configs/StatesMonitorConfig.h
@@ -0,0 +1,38 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace StatesMonitor
+{
+constexpr int BOARDS_NUMBER         = 5;
+constexpr long long int MAX_TIMEOUT = 5000;  // [ms]
+constexpr uint32_t UPDATE_PERIOD    = 1000;
+}  // namespace StatesMonitor
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Configs/TARSConfig.h b/src/boards/RIG/Configs/TARSConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6031a3053a64e5986c3eae627b796a476e32c1c
--- /dev/null
+++ b/src/boards/RIG/Configs/TARSConfig.h
@@ -0,0 +1,48 @@
+/* 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.
+ */
+#pragma once
+
+#include <stdint.h>
+
+namespace RIG
+{
+namespace Config
+{
+namespace TARS
+{
+constexpr uint16_t TARS_UPDATE_PERIOD       = 10;
+constexpr uint8_t TARS_FILTER_SAMPLE_NUMBER = 10;
+constexpr float TARS_TARGET_TEMPERATURE     = 25;  // Celsius
+
+constexpr uint32_t TARS_WASHING_OPENING_TIME = 5000;  // [ms]
+constexpr uint32_t TARS_WASHING_TIME_DELAY =
+    1000;  //[ms] Represents a delay of opening two different valves
+constexpr uint32_t TARS_FILLING_OPENING_TIME         = 900000;  // [ms] 15min
+constexpr uint32_t TARS_PRESSURE_STABILIZE_WAIT_TIME = 1000;    // [ms] 1s
+
+constexpr uint16_t TARS_NUMBER_MASS_STABLE_ITERATIONS = 2;
+
+constexpr float TARS_MASS_TOLERANCE     = 0.2;    //[kg]
+constexpr float TARS_PRESSURE_TOLERANCE = 0.035;  //[bar]
+}  // namespace TARS
+}  // namespace Config
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Radio/Radio.cpp b/src/boards/RIG/Radio/Radio.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3febc05c03b92e5878092eec5f7a67abf165b43b
--- /dev/null
+++ b/src/boards/RIG/Radio/Radio.cpp
@@ -0,0 +1,455 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/Buses.h>
+#include <RIG/CanHandler/CanHandler.h>
+#include <RIG/Radio/Radio.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIG/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 <radio/SerialTransceiver/SerialTransceiver.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+void __attribute__((used)) EXTI5_IRQHandlerImpl()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<RIG::Radio>()->transceiver != nullptr)
+    {
+        modules.get<RIG::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) EXTI12_IRQHandlerImpl()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<RIG::Radio>()->transceiver != nullptr)
+    {
+        modules.get<RIG::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+
+void __attribute__((used)) EXTI13_IRQHandlerImpl()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    if (modules.get<RIG::Radio>()->transceiver != nullptr)
+    {
+        modules.get<RIG::Radio>()->transceiver->handleDioIRQ();
+    }
+}
+namespace RIG
+{
+
+Radio::Radio()
+{
+    // Initialize previous state to avoid undefined behaviour
+    previousState.arm_switch           = 0;
+    previousState.filling_valve_btn    = 0;
+    previousState.ignition_btn         = 0;
+    previousState.quick_connector_btn  = 0;
+    previousState.release_pressure_btn = 0;
+    previousState.start_tars_btn       = 0;
+    previousState.venting_valve_btn    = 0;
+}
+
+Radio::~Radio()
+{
+    mavDriver->stop();
+    delete mavDriver;
+}
+
+bool Radio::start()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Config the transceiver
+    SX1278Lora::Config config{};
+    config.power            = 2;
+    config.ocp              = 0;  // Over current protection
+    config.coding_rate      = SX1278Lora::Config::Cr::CR_1;
+    config.spreading_factor = SX1278Lora::Config::Sf::SF_7;
+
+    std::unique_ptr<SX1278::ISX1278Frontend> frontend =
+        std::make_unique<EbyteFrontend>(radio::txEn::getPin(),
+                                        radio::rxEn::getPin());
+
+    transceiver = new SX1278Lora(
+        modules.get<Buses>()->spi4, radio::cs::getPin(), radio::dio0::getPin(),
+        radio::dio1::getPin(), radio::dio3::getPin(), SPI::ClockDivider::DIV_64,
+        std::move(frontend));
+
+    SX1278Lora::Error error = transceiver->init(config);
+
+    // Config mavDriver
+    mavDriver = new MavDriver(
+        transceiver,
+        [=](MavDriver*, const mavlink_message_t& msg)
+        { this->handleMavlinkMessage(msg); },
+        Config::Radio::MAV_SLEEP_AFTER_SEND,
+        Config::Radio::MAV_OUT_BUFFER_MAX_AGE);
+
+    // This thread allows the radio to exit a possible deadlock situation where
+    // the driver waits for a missed interrupt. By default the priority is MAX -
+    // 1
+    radioBackupDIO = std::thread(
+        [=]()
+        {
+            while (true)
+            {
+                Thread::sleep(
+                    5000);  // wait 5 seconds and then launch the interrupt
+                {
+                    FastInterruptDisableLock lock;
+                    transceiver->handleDioIRQ();
+                }
+            }
+        });
+
+    // In case of failure the mav driver must be created at least
+    if (error != SX1278Lora::Error::NONE)
+    {
+        return false;
+    }
+
+    return mavDriver->start();
+}
+
+void Radio::sendAck(const mavlink_message_t& msg)
+{
+    mavlink_message_t ackMsg;
+    mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                            Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid,
+                            msg.seq);
+    buffer.put(ackMsg);
+}
+
+void Radio::sendNack(const mavlink_message_t& msg)
+{
+    mavlink_message_t nackMsg;
+    mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                             Config::Radio::MAV_COMPONENT_ID, &nackMsg,
+                             msg.msgid, msg.seq);
+    buffer.put(nackMsg);
+}
+
+void Radio::logStatus()
+{
+    MavlinkStatus stats = mavDriver->getStatus();
+    SDlogger.log(stats);
+}
+
+bool Radio::isStarted() { return mavDriver->isStarted(); }
+
+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:
+        {
+            // Important to return instead of breaking because handle command
+            // can decide whether to send ack or nack
+            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));
+            switch (tmId)
+            {
+                default:
+                {
+                    mavlink_message_t response =
+                        modules.get<TMRepository>()->packSystemTm(
+                            tmId, msg.msgid, msg.seq);
+
+                    // Add the response to the buffer
+                    buffer.put(response);
+
+                    if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
+                    {
+                        // No break to let tmRepo decide for nack
+                        return;
+                    }
+                    break;
+                }
+            }
+            break;
+        }
+        case MAVLINK_MSG_ID_CONRIG_STATE_TC:
+        {
+            // In this case all the messages in the queue must be sent
+            // after adding the ack to the queue. DO NOT USE get AND isFull with
+            // the generic message class type
+            sendAck(msg);
+            size_t count = buffer.count();
+
+            for (size_t i = 0; i < count; i++)
+            {
+                // Handle the possible exception
+                try
+                {
+                    mavDriver->enqueueMsg(buffer.pop());
+                }
+                catch (const std::exception& e)
+                {
+                    LOG_ERR(logger, "Circular buffer generated an exception");
+                }
+            }
+
+            // After sending all the packets in buffer send the RIG state
+            // messages
+            mavDriver->enqueueMsg(modules.get<TMRepository>()->packSystemTm(
+                SystemTMList::MAV_GSE_ID, msg.msgid, msg.seq));
+            mavDriver->enqueueMsg(modules.get<TMRepository>()->packSystemTm(
+                SystemTMList::MAV_MOTOR_ID, msg.msgid, msg.seq));
+
+            mavlink_conrig_state_tc_t state;
+            mavlink_msg_conrig_state_tc_decode(&msg, &state);
+
+            // Extract the buttons data and if there is a slope post the event
+            if (previousState.arm_switch == 0 && state.arm_switch == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    EventBroker::getInstance().post(Common::TMTC_ARM,
+                                                    Common::TOPIC_MOTOR);
+                    modules.get<CanHandler>()->sendEvent(
+                        Common::CanConfig::EventId::ARM);
+
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.filling_valve_btn == 0 &&
+                state.filling_valve_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    modules.get<Actuators>()->toggleServo(
+                        ServosList::FILLING_VALVE);
+
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.ignition_btn == 0 && state.ignition_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    EventBroker::getInstance().post(Common::MOTOR_IGNITION,
+                                                    Common::TOPIC_MOTOR);
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.quick_connector_btn == 0 &&
+                state.quick_connector_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    modules.get<Actuators>()->toggleServo(
+                        ServosList::DISCONNECT_SERVO);
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.release_pressure_btn == 0 &&
+                state.release_pressure_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    modules.get<Actuators>()->toggleServo(
+                        ServosList::RELEASE_VALVE);
+
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.start_tars_btn == 0 && state.start_tars_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_START_TARS,
+                                                    Common::TOPIC_TARS);
+                    lastManualCommand = getTick();
+                }
+            }
+            if (previousState.venting_valve_btn == 0 &&
+                state.venting_valve_btn == 1)
+            {
+                if (getTick() > lastManualCommand +
+                                    Config::Radio::RADIO_LAST_COMMAND_THRESHOLD)
+                {
+                    EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                    Common::TOPIC_TARS);
+                    modules.get<Actuators>()->toggleServo(
+                        ServosList::VENTING_VALVE);
+
+                    lastManualCommand = getTick();
+                }
+            }
+
+            if (previousState.arm_switch == 1 && state.arm_switch == 0)
+            {
+                EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                                Common::TOPIC_TARS);
+                EventBroker::getInstance().post(Common::TMTC_DISARM,
+                                                Common::TOPIC_MOTOR);
+                modules.get<CanHandler>()->sendEvent(
+                    Common::CanConfig::EventId::DISARM);
+
+                lastManualCommand = getTick();
+            }
+
+            previousState = state;
+
+            // Important to return and not to break to avoid a redundant ack
+            return;
+        }
+        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+        {
+            EventBroker::getInstance().post(Common::MOTOR_MANUAL_ACTION,
+                                            Common::TOPIC_TARS);
+            // Wiggle only the machine is in ready state
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+            if (modules.get<GroundModeManager>()->getStatus().state ==
+                GroundModeManagerState::READY)
+            {
+                bool result = modules.get<Actuators>()->wiggleServo(servo);
+
+                if (!result)
+                {
+                    sendNack(msg);
+                    return;
+                }
+            }
+            else
+            {
+                sendNack(msg);
+                return;
+            }
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_ATOMIC_VALVE_TIMING_TC:
+        {
+            uint32_t timing =
+                mavlink_msg_set_atomic_valve_timing_tc_get_maximum_timing(&msg);
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_set_atomic_valve_timing_tc_get_servo_id(&msg));
+            modules.get<Actuators>()->setTiming(servo, timing);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_VALVE_MAXIMUM_APERTURE_TC:
+        {
+            float aperture =
+                mavlink_msg_set_valve_maximum_aperture_tc_get_maximum_aperture(
+                    &msg);
+            ServosList servo = static_cast<ServosList>(
+                mavlink_msg_set_valve_maximum_aperture_tc_get_servo_id(&msg));
+            modules.get<Actuators>()->setMaximumAperture(servo, aperture);
+            break;
+        }
+        case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC:
+        {
+            uint32_t timing = mavlink_msg_set_ignition_time_tc_get_timing(&msg);
+            modules.get<GroundModeManager>()->setIgnitionTime(timing);
+            break;
+        }
+        default:
+        {
+            // Unknown message
+            sendNack(msg);
+            return;
+        }
+    }
+    sendAck(msg);
+}
+
+void Radio::handleCommand(const mavlink_message_t& msg)
+{
+    uint8_t command = mavlink_msg_command_tc_get_command_id(&msg);
+    switch (command)
+    {
+        case MAV_CMD_START_LOGGING:
+        {
+            bool result = Logger::getInstance().start();
+
+            // If the log reached the maximum allowed number then send a nack
+            if (!result || Logger::getInstance().getCurrentLogNumber() ==
+                               Logger::getMaxFilenameNumber())
+            {
+                sendNack(msg);
+                return;
+            }
+
+            break;
+        }
+        case MAV_CMD_STOP_LOGGING:
+        {
+            Logger::getInstance().stop();
+            break;
+        }
+        case MAV_CMD_CALIBRATE:
+        {
+            EventBroker::getInstance().post(Common::TMTC_CALIBRATE,
+                                            Common::TOPIC_MOTOR);
+            ModuleManager::getInstance().get<CanHandler>()->sendEvent(
+                Common::CanConfig::EventId::CALIBRATE);
+            break;
+        }
+        default:
+        {
+            // Send a nack for unknown message
+            sendNack(msg);
+            return;
+        }
+    }
+
+    sendAck(msg);
+}
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Radio/Radio.h b/src/boards/RIG/Radio/Radio.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f91e498ed11d0e89f11f46965523013103212c6
--- /dev/null
+++ b/src/boards/RIG/Radio/Radio.h
@@ -0,0 +1,102 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <RIG/Configs/RadioConfig.h>
+#include <common/Mavlink.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SX1278/SX1278Lora.h>
+#include <utils/collections/CircularBuffer.h>
+
+#include <thread>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Lora::MTU,
+                                           Config::Radio::RADIO_OUT_QUEUE_SIZE,
+                                           Config::Radio::RADIO_MAV_MSG_LENGTH>;
+class Radio : public Boardcore::Module
+{
+public:
+    Radio();
+
+    ~Radio();
+
+    /**
+     * @brief Starts the MavlinkDriver.
+     */
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Prepares and adds to the buffer an ack message for the given
+     * message.
+     *
+     * @param msg The message received that we need to acknowledge.
+     */
+    void sendAck(const mavlink_message_t& msg);
+
+    /**
+     * @brief Prepares and adds to the buffer a nack message for the given
+     * message.
+     *
+     * @param msg The message received that we need to not acknowledge.
+     */
+    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::SX1278Lora* 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 handleMavlinkMessage to handle a command message.
+     */
+    void handleCommand(const mavlink_message_t& msg);
+
+    mavlink_conrig_state_tc_t previousState;
+    Boardcore::CircularBuffer<mavlink_message_t,
+                              Config::Radio::RADIO_CIRCULAR_BUFFER_SIZE>
+        buffer;
+    std::thread radioBackupDIO;
+
+    long long int lastManualCommand =
+        0;  // Specifies the last tick [ms] in which a command is executed
+    Boardcore::Logger& SDlogger   = Boardcore::Logger::getInstance();
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Sensors/ADCsData.h b/src/boards/RIG/Sensors/ADCsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..464f702fefd458f323c77af60749977d0e6602e3
--- /dev/null
+++ b/src/boards/RIG/Sensors/ADCsData.h
@@ -0,0 +1,55 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <sensors/ADS131M04/ADS131M04Data.h>
+
+namespace RIG
+{
+struct ADCsData : Boardcore::ADS131M04Data
+{
+    uint8_t ADCnumber;
+
+    ADCsData() : ADS131M04Data{0, 0, 0, 0, 0} { ADCnumber = 0; }
+
+    ADCsData(uint64_t time, uint8_t num, float ch1, float ch2, float ch3,
+             float ch4)
+        : ADS131M04Data{time, ch1, ch2, ch3, ch4}
+    {
+        ADCnumber = num;
+    }
+
+    static std::string header()
+    {
+        return "timestamp,ADCnumber,voltage_channel_1,voltage_channel_2,"
+               "voltage_channel_"
+               "3,voltage_channel_4\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)ADCnumber << "," << voltage[0] << ","
+           << voltage[1] << "," << voltage[2] << "," << voltage[3] << "\n";
+    }
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Sensors/LoadCellsData.h b/src/boards/RIG/Sensors/LoadCellsData.h
new file mode 100644
index 0000000000000000000000000000000000000000..6628bf0aafaac7b778d9281b2dddce9d886231e9
--- /dev/null
+++ b/src/boards/RIG/Sensors/LoadCellsData.h
@@ -0,0 +1,50 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <sensors/SensorData.h>
+#include <stdint.h>
+
+namespace RIG
+{
+struct LoadCellsData : Boardcore::LoadCellData
+{
+    uint8_t cellNumber;
+
+    LoadCellsData() : LoadCellData{0, 0} { cellNumber = 0; }
+
+    LoadCellsData(uint64_t time, uint8_t cell, float l)
+        : Boardcore::LoadCellData{time, l}
+    {
+        cellNumber = cell;
+    }
+
+    static std::string header() { return "loadTimestamp,cellNumber,load\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << loadTimestamp << "," << (int)cellNumber << "," << load << ","
+           << "\n";
+    }
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Sensors/Sensors.cpp b/src/boards/RIG/Sensors/Sensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96ecbd2648e0edc2378a137ac0635a2dc50a7c07
--- /dev/null
+++ b/src/boards/RIG/Sensors/Sensors.cpp
@@ -0,0 +1,491 @@
+/* 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 <RIG/Buses.h>
+#include <RIG/Configs/SensorsConfig.h>
+#include <RIG/Sensors/ADCsData.h>
+#include <RIG/Sensors/LoadCellsData.h>
+#include <RIG/Sensors/Sensors.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace std;
+
+namespace RIG
+{
+ADS131M04Data Sensors::getADC1LastSample()
+{
+    PauseKernelLock l;
+    return adc1->getLastSample();
+}
+
+ADS131M04Data Sensors::getADC2LastSample()
+{
+    PauseKernelLock l;
+    return adc2->getLastSample();
+}
+
+HX711Data Sensors::getTankWeight()
+{
+    PauseKernelLock l;
+    HX711Data sample = loadCell2->getLastSample();
+    sample.load -= offsetLoadCell2;
+    sample.load = -sample.load;
+    return sample;
+}
+
+HX711Data Sensors::getVesselWeight()
+{
+    PauseKernelLock l;
+    HX711Data sample = loadCell1->getLastSample();
+    sample.load -= offsetLoadCell1;
+    return sample;
+}
+
+TemperatureData Sensors::getThermocoupleLastSample()
+{
+    PauseKernelLock l;
+    return ((!canSensorFlag) ? thermocouple->getLastSample()
+                             : canTankTemperature);
+}
+
+PressureData Sensors::getFillingPress()
+{
+    // It is important to call the getter method to separate the critical
+    // section from the calculation ones
+    ADS131M04Data lastSample = getADC1LastSample();
+
+    // Calculate the current and then based on linear relation calculate the
+    // pressure
+    float current =
+        lastSample.voltage[1] / Config::Sensors::ADC1_CH1_SHUNT_RESISTANCE;
+
+    // Convert the current in mA
+    current = current * 1000;
+
+    // Calculate the pressure given the linear formula 4..20mA for
+    // 0..max_pressure
+
+    // shift the current to let the line pass through 0 point
+    current = current - Config::Sensors::SENSOR_MIN_CURRENT;
+
+    float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT -
+                                 Config::Sensors::SENSOR_MIN_CURRENT)) *
+                     Config::Sensors::FILLING_MAX_PRESSURE;
+
+    return PressureData{lastSample.timestamp, pressure};
+}
+
+PressureData Sensors::getTankTopPress()
+{
+    // It is important to call the getter method to separate the critical
+    // section from the calculation ones
+    ADS131M04Data lastSample = getADC1LastSample();
+
+    // Calculate the current and then based on linear relation calculate the
+    // pressure
+    float current =
+        lastSample.voltage[3] / Config::Sensors::ADC1_CH2_SHUNT_RESISTANCE;
+
+    // Convert the current in mA
+    current = current * 1000;
+
+    // Calculate the pressure given the linear formula 4..20mA for
+    // 0..max_pressure
+
+    // shift the current to let the line pass through 0 point
+    current = current - Config::Sensors::SENSOR_MIN_CURRENT;
+
+    float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT -
+                                 Config::Sensors::SENSOR_MIN_CURRENT)) *
+                     Config::Sensors::TANK_TOP_MAX_PRESSURE;
+
+    return ((!canSensorFlag) ? PressureData{lastSample.timestamp, pressure}
+                             : canTopTankPressure);
+}
+
+PressureData Sensors::getTankBottomPress()
+{
+    // It is important to call the getter method to separate the critical
+    // section from the calculation ones
+    ADS131M04Data lastSample = getADC1LastSample();
+
+    // Calculate the current and then based on linear relation calculate the
+    // pressure
+    float current =
+        lastSample.voltage[2] / Config::Sensors::ADC1_CH3_SHUNT_RESISTANCE;
+
+    // Convert the current in mA
+    current = current * 1000;
+
+    // Calculate the pressure given the linear formula 4..20mA for
+    // 0..max_pressure
+
+    // shift the current to let the line pass through 0 point
+    current = current - Config::Sensors::SENSOR_MIN_CURRENT;
+
+    float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT -
+                                 Config::Sensors::SENSOR_MIN_CURRENT)) *
+                     Config::Sensors::TANK_BOTTOM_MAX_PRESSURE;
+
+    return ((!canSensorFlag) ? PressureData{lastSample.timestamp, pressure}
+                             : canBottomTankPressure);
+}
+
+PressureData Sensors::getVesselPress()
+{
+    // It is important to call the getter method to separate the critical
+    // section from the calculation ones
+    ADS131M04Data lastSample = getADC1LastSample();
+
+    // Calculate the current and then based on linear relation calculate the
+    // pressure
+    float current =
+        lastSample.voltage[0] / Config::Sensors::ADC1_CH4_SHUNT_RESISTANCE;
+
+    // Convert the current in mA
+    current = current * 1000;
+
+    // Calculate the pressure given the linear formula 4..20mA for
+    // 0..max_pressure
+
+    // shift the current to let the line pass through 0 point
+    current = current - Config::Sensors::SENSOR_MIN_CURRENT;
+
+    float pressure = (current / (Config::Sensors::SENSOR_MAX_CURRENT -
+                                 Config::Sensors::SENSOR_MIN_CURRENT)) *
+                     Config::Sensors::VESSEL_MAX_PRESSURE;
+
+    return PressureData{lastSample.timestamp, pressure};
+}
+
+BatteryVoltageSensorData Sensors::getBatteryVoltage()
+{
+    // Get the analog ADC2 reading
+    ADS131M04Data lastSample = getADC2LastSample();
+
+    // Raw reading
+    float voltage =
+        Config::Sensors::VOLTAGE_CONVERSION_FACTOR * lastSample.voltage[2];
+
+    BatteryVoltageSensorData result{};
+
+    result.voltage          = voltage;
+    result.voltageTimestamp = lastSample.timestamp;
+    return result;
+}
+
+CurrentData Sensors::getCurrent()
+{
+    // Get the analog ADC2 reading
+    ADS131M04Data lastSample = getADC2LastSample();
+
+    // Raw voltage reading
+    float voltage =
+        Config::Sensors::CURRENT_CONVERSION_FACTOR * lastSample.voltage[3];
+
+    // 2.5V is half the dynamic
+    float current = -((voltage - 2.52) /
+                      Config::Sensors::VOLTAGE_CURRENT_CONVERSION_FACTOR);
+
+    CurrentData result{};
+
+    result.current          = current;
+    result.currentTimestamp = lastSample.timestamp;
+    return result;
+}
+
+PressureData Sensors::getCCPress()
+{
+    PauseKernelLock lock;
+    return canCCPressure;
+}
+
+CurrentData Sensors::getMainCurrent()
+{
+    PauseKernelLock lock;
+    return (!canSensorFlag) ? CurrentData{} : canMainCurrent;
+}
+
+CurrentData Sensors::getPayloadCurrent()
+{
+    PauseKernelLock lock;
+    return (!canSensorFlag) ? CurrentData{} : canPayloadCurrent;
+}
+
+CurrentData Sensors::getMotorCurrent()
+{
+    PauseKernelLock lock;
+    return (!canSensorFlag) ? CurrentData{} : canMotorCurrent;
+}
+
+BatteryVoltageSensorData Sensors::getMotorBatteryVoltage()
+{
+    PauseKernelLock lock;
+    return (!canSensorFlag) ? BatteryVoltageSensorData{} : canMotorVoltage;
+}
+
+void Sensors::setCCPressure(PressureData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                   = true;
+    canCCPressure                   = data;
+    canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setBottomTankPressure(PressureData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                           = true;
+    canBottomTankPressure                   = data;
+    canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setTopTankPressure(PressureData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                        = true;
+    canTopTankPressure                   = data;
+    canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setTankTemperature(TemperatureData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                           = true;
+    canTankTemperature                      = data;
+    canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setMotorCurrent(CurrentData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                    = true;
+    canMotorCurrent                  = data;
+    canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setMainCurrent(CurrentData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                   = true;
+    canMainCurrent                  = data;
+    canMainCurrent.currentTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setPayloadCurrent(CurrentData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                      = true;
+    canPayloadCurrent                  = data;
+    canPayloadCurrent.currentTimestamp = TimestampTimer::getTimestamp();
+}
+
+void Sensors::setMotorVoltage(BatteryVoltageSensorData data)
+{
+    PauseKernelLock lock;
+    canSensorFlag                    = true;
+    canMotorVoltage                  = data;
+    canMotorVoltage.voltageTimestamp = TimestampTimer::getTimestamp();
+}
+
+Sensors::Sensors(TaskScheduler* sched) : scheduler{sched} {}
+
+bool Sensors::start()
+{
+    // Init all the sensors
+    adc1Init();
+    adc2Init();
+    loadCell1Init();
+    loadCell2Init();
+    thermocoupleInit();
+
+    // Create the sensors manager with the desired scheduler and the sensors map
+    manager = new SensorManager(sensorMap, scheduler);
+    return manager->start();
+}
+
+void Sensors::stop() { manager->stop(); }
+
+bool Sensors::isStarted() { return manager->areAllSensorsInitialized(); }
+
+void Sensors::calibrate()
+{
+    Stats sample1, sample2;
+
+    // Take some samples of the 2 load cells
+    for (int i = 0; i < 10; i++)
+    {
+        // DO NOT USE THE GETTERS BECAUSE THE OFFSET IS ALREADY APPLIED
+        {
+            PauseKernelLock lock;
+            sample1.add(loadCell1->getLastSample().load);
+            sample2.add(loadCell2->getLastSample().load);
+        }
+
+        // Wait for some reasonable time
+        Thread::sleep(Config::Sensors::LOAD_CELL_SAMPLE_PERIOD * 3);
+    }
+
+    offsetLoadCell1 = sample1.getStats().mean;
+    offsetLoadCell2 = sample2.getStats().mean;
+}
+
+void Sensors::adc1Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    SPIBusConfig config;
+    config.mode         = SPI::Mode::MODE_0;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+
+    ADS131M04::Config sensorConfig;
+    sensorConfig.oversamplingRatio = ADS131M04Defs::OversamplingRatio::OSR_8192;
+    sensorConfig.globalChopModeEnabled = true;
+
+    adc1 = new ADS131M04(modules.get<Buses>()->spi1,
+                         sensors::ADS131_1::cs::getPin(), config, sensorConfig);
+
+    // Config and enter the sensors info to feed to the sensors manager
+    SensorInfo info("ADS131_1", Config::Sensors::ADC_SAMPLE_PERIOD,
+                    bind(&Sensors::adc1Callback, this));
+    sensorMap.emplace(make_pair(adc1, info));
+    LOG_INFO(logger, "Initialized ADC1");
+}
+void Sensors::adc1Callback()
+{
+    ADS131M04Data sample = adc1->getLastSample();
+
+    ADCsData data{sample.timestamp,  1,
+                  sample.voltage[0], sample.voltage[1],
+                  sample.voltage[2], sample.voltage[3]};
+
+    SDlogger.log(data);
+}
+
+void Sensors::adc2Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    SPIBusConfig config;
+    config.mode         = SPI::Mode::MODE_0;
+    config.clockDivider = SPI::ClockDivider::DIV_32;
+
+    ADS131M04::Config sensorConfig;
+    sensorConfig.oversamplingRatio = ADS131M04Defs::OversamplingRatio::OSR_8192;
+    sensorConfig.globalChopModeEnabled = true;
+
+    adc2 = new ADS131M04(modules.get<Buses>()->spi1,
+                         sensors::ADS131_2::cs::getPin(), config, sensorConfig);
+
+    // Config and enter the sensors info to feed to the sensors manager
+    SensorInfo info("ADS131_2", Config::Sensors::ADC_SAMPLE_PERIOD,
+                    bind(&Sensors::adc2Callback, this));
+    sensorMap.emplace(make_pair(adc2, info));
+    LOG_INFO(logger, "Initialized ADC2");
+}
+void Sensors::adc2Callback()
+{
+    ADS131M04Data sample = adc2->getLastSample();
+
+    ADCsData data{sample.timestamp,  2,
+                  sample.voltage[0], sample.voltage[1],
+                  sample.voltage[2], sample.voltage[3]};
+
+    SDlogger.log(data);
+}
+
+void Sensors::loadCell1Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    loadCell1 =
+        new HX711(modules.get<Buses>()->spi2, sensors::HX711_1::sck::getPin());
+
+    loadCell1->setOffset(Config::Sensors::LOAD_CELL1_OFFSET);
+    loadCell1->setScale(Config::Sensors::LOAD_CELL1_SCALE);
+
+    // Config and enter the sensors info to feed to the sensors manager
+    SensorInfo info("HX711_1", Config::Sensors::LOAD_CELL_SAMPLE_PERIOD,
+                    bind(&Sensors::loadCell1Callback, this));
+    sensorMap.emplace(make_pair(loadCell1, info));
+    LOG_INFO(logger, "Initialized loadCell1");
+}
+void Sensors::loadCell1Callback()
+{
+    HX711Data sample = loadCell1->getLastSample();
+
+    LoadCellsData data;
+
+    data.cellNumber    = 1;
+    data.load          = sample.load;
+    data.loadTimestamp = sample.loadTimestamp;
+
+    SDlogger.log(data);
+}
+
+void Sensors::loadCell2Init()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    loadCell2 =
+        new HX711(modules.get<Buses>()->spi6, sensors::HX711_2::sck::getPin());
+
+    loadCell2->setOffset(Config::Sensors::LOAD_CELL2_OFFSET);
+    loadCell2->setScale(Config::Sensors::LOAD_CELL2_SCALE);
+
+    // Config and enter the sensors info to feed to the sensors manager
+    SensorInfo info("HX711_2", Config::Sensors::LOAD_CELL_SAMPLE_PERIOD,
+                    bind(&Sensors::loadCell2Callback, this));
+    sensorMap.emplace(make_pair(loadCell2, info));
+    LOG_INFO(logger, "Initialized loadCell2");
+}
+void Sensors::loadCell2Callback()
+{
+    HX711Data sample = loadCell2->getLastSample();
+
+    LoadCellsData data;
+
+    data.cellNumber    = 2;
+    data.load          = sample.load;
+    data.loadTimestamp = sample.loadTimestamp;
+
+    SDlogger.log(data);
+}
+
+void Sensors::thermocoupleInit()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    thermocouple           = new MAX31855(modules.get<Buses>()->spi1,
+                                          sensors::MAX31855::cs::getPin());
+
+    // Config and enter the sensors info to feed to the sensors manager
+    SensorInfo info("MAX31855", Config::Sensors::THERMOCOUPLE_SAMPLE_PERIOD,
+                    bind(&Sensors::thermocoupleCallback, this));
+    sensorMap.emplace(make_pair(thermocouple, info));
+    LOG_INFO(logger, "Initialized thermocouple");
+}
+void Sensors::thermocoupleCallback()
+{
+    SDlogger.log(thermocouple->getLastSample());
+}
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/Sensors/Sensors.h b/src/boards/RIG/Sensors/Sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..de7b1742b4c9e8c61d96cc39c113de91376707b5
--- /dev/null
+++ b/src/boards/RIG/Sensors/Sensors.h
@@ -0,0 +1,151 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <sensors/ADS131M04/ADS131M04.h>
+#include <sensors/HX711/HX711.h>
+#include <sensors/MAX31855/MAX31855.h>
+#include <sensors/SensorManager.h>
+#include <sensors/analog/BatteryVoltageSensorData.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+class Sensors : public Boardcore::Module
+{
+public:
+    /**
+     * @brief Construct a new Sensors object
+     *
+     * @param sched The scheduler to be used from the board scheduler
+     * @note The scheduler determines the priority
+     */
+    Sensors(Boardcore::TaskScheduler* sched);
+
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Stops the sensors manager
+     * @warning Stops the passed scheduler
+     */
+    void stop();
+
+    /**
+     * @brief Returns if the module is correctly started
+     */
+    bool isStarted();
+
+    /**
+     * @brief Calibrates the load cells with an offset
+     */
+    void calibrate();
+
+    // Getters methods, they pause the kernel
+    Boardcore::ADS131M04Data getADC1LastSample();
+    Boardcore::ADS131M04Data getADC2LastSample();
+    Boardcore::HX711Data getTankWeight();
+    Boardcore::HX711Data getVesselWeight();
+    Boardcore::TemperatureData getThermocoupleLastSample();
+
+    // Processed getters
+    Boardcore::PressureData getFillingPress();
+    Boardcore::PressureData getTankTopPress();
+    Boardcore::PressureData getTankBottomPress();
+    Boardcore::PressureData getVesselPress();
+    Boardcore::BatteryVoltageSensorData getBatteryVoltage();
+    Boardcore::CurrentData getCurrent();
+    Boardcore::PressureData getCCPress();
+
+    // CAN sensors setters
+    void setCCPressure(Boardcore::PressureData data);
+    void setBottomTankPressure(Boardcore::PressureData data);
+    void setTopTankPressure(Boardcore::PressureData data);
+    void setTankTemperature(Boardcore::TemperatureData data);
+    void setMotorCurrent(Boardcore::CurrentData data);
+    void setMainCurrent(Boardcore::CurrentData data);
+    void setPayloadCurrent(Boardcore::CurrentData data);
+    void setMotorVoltage(Boardcore::BatteryVoltageSensorData data);
+
+    // Can sensors getters
+    Boardcore::CurrentData getMotorCurrent();
+    Boardcore::CurrentData getMainCurrent();
+    Boardcore::CurrentData getPayloadCurrent();
+    Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage();
+
+private:
+    // Init and callback methods
+    void adc1Init();
+    void adc1Callback();
+
+    void adc2Init();
+    void adc2Callback();
+
+    void loadCell1Init();
+    void loadCell1Callback();
+
+    void loadCell2Init();
+    void loadCell2Callback();
+
+    void thermocoupleInit();
+    void thermocoupleCallback();
+
+    // SD logger
+    Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance();
+
+    // Sensors
+    Boardcore::ADS131M04* adc1 = nullptr;
+    Boardcore::ADS131M04* adc2 = nullptr;
+
+    Boardcore::HX711* loadCell1 = nullptr;
+    Boardcore::HX711* loadCell2 = nullptr;
+
+    Boardcore::MAX31855* thermocouple = nullptr;
+
+    // Manager components
+    Boardcore::SensorManager* manager = nullptr;
+    Boardcore::SensorManager::SensorMap_t sensorMap;
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // CAN Sensors flag. By default the RIG outputs the sensors from the on
+    // board ADC, but as soon as a CAN sensor arrives, the sensors are switched
+    // from the CAN
+    bool canSensorFlag = false;
+
+    // CAN fake sensors data
+    Boardcore::PressureData canCCPressure;
+    Boardcore::PressureData canBottomTankPressure;
+    Boardcore::PressureData canTopTankPressure;
+    Boardcore::TemperatureData canTankTemperature;
+    Boardcore::CurrentData canMotorCurrent;
+    Boardcore::CurrentData canMainCurrent;
+    Boardcore::CurrentData canPayloadCurrent;
+    Boardcore::BatteryVoltageSensorData canMotorVoltage;
+
+    // Offsets (thread safe)
+    std::atomic<float> offsetLoadCell1{0.0};
+    std::atomic<float> offsetLoadCell2{0.0};
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors");
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..36e9140c3c8477ac11520e91d8d79c7ea382ab7b
--- /dev/null
+++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -0,0 +1,199 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/CanHandler/CanHandler.h>
+#include <RIG/Configs/GroundModeManagerConfig.h>
+#include <RIG/Sensors/Sensors.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace Common;
+
+namespace RIG
+{
+GroundModeManager::GroundModeManager()
+    : FSM(&GroundModeManager::state_idle),
+      ignitionTime(Config::Ignition::DEFAULT_IGNITION_WAITING_TIME)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+}
+
+void GroundModeManager::state_idle(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(GroundModeManagerState::IDLE);
+        }
+        case FMM_INIT_OK:
+        {
+            return transition(&GroundModeManager::state_ready);
+        }
+    }
+}
+
+void GroundModeManager::state_ready(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Send an DISARM CAN event
+            modules.get<CanHandler>()->sendEvent(
+                Common::CanConfig::EventId::DISARM);
+
+            // Disarm the relays
+            miosix::relays::ignition::high();
+            miosix::relays::nitrogen::high();
+            miosix::relays::ledLamp::high();
+
+            return logStatus(GroundModeManagerState::READY);
+        }
+        case TMTC_ARM:
+        {
+            return transition(&GroundModeManager::state_armed);
+        }
+        case TMTC_CALIBRATE:
+        {
+            // Send CAN event
+            modules.get<CanHandler>()->sendEvent(
+                Common::CanConfig::EventId::CALIBRATE);
+
+            // Blocking (but not too complex) operation of calibration
+            modules.get<Sensors>()->calibrate();
+            break;
+        }
+    }
+}
+
+void GroundModeManager::state_armed(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Send an ARM CAN event
+            modules.get<CanHandler>()->sendEvent(
+                Common::CanConfig::EventId::ARM);
+
+            // Turn on the light
+            miosix::relays::ledLamp::low();
+
+            // Expire all servos timings
+            modules.get<Actuators>()->closeAllServo();
+            return logStatus(GroundModeManagerState::ARMED);
+        }
+        case TMTC_DISARM:
+        {
+            // Turn off the light
+            miosix::relays::ledLamp::high();
+
+            return transition(&GroundModeManager::state_ready);
+        }
+        case MOTOR_IGNITION:
+        {
+            return transition(&GroundModeManager::state_igniting);
+        }
+    }
+}
+
+void GroundModeManager::state_igniting(const Event& event)
+{
+    // A static variable is not really necessary
+    static int oxidantTimeoutEventId = -1;
+
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Fire the igniter
+            miosix::relays::ignition::low();
+
+            // Set the delayed event for oxidant
+            oxidantTimeoutEventId = EventBroker::getInstance().postDelayed(
+                MOTOR_OPEN_OXIDANT, TOPIC_MOTOR, ignitionTime);
+
+            return logStatus(GroundModeManagerState::IGNITING);
+        }
+        case MOTOR_OPEN_OXIDANT:
+        {
+            // Shut down the igniter
+            miosix::relays::ignition::high();
+
+            modules.get<Actuators>()->toggleServo(ServosList::MAIN_VALVE);
+            break;
+        }
+        case MOTOR_CLOSE_FEED_VALVE:
+        case TMTC_DISARM:
+        {
+            // Shut down the igniter
+            miosix::relays::ignition::high();
+
+            // Close all the valves
+            modules.get<Actuators>()->closeAllServo();
+
+            EventBroker::getInstance().removeDelayed(oxidantTimeoutEventId);
+
+            for (uint32_t i = 0; i < Config::Ignition::NUMBER_NITROGEN_BUMPS;
+                 i++)
+            {
+                Thread::sleep(Config::Ignition::DEAD_TIME_AFTER_BURN);
+                // Open nitrogen
+                miosix::relays::nitrogen::low();
+                Thread::sleep(Config::Ignition::NITROGEN_OPENING_TIME);
+                // Close nitrogen
+                miosix::relays::nitrogen::high();
+            }
+
+            return transition(&GroundModeManager::state_ready);
+        }
+    }
+}
+
+void GroundModeManager::setIgnitionTime(uint32_t ignitionTime)
+{
+    this->ignitionTime = ignitionTime;
+}
+
+void GroundModeManager::logStatus(GroundModeManagerState state)
+{
+    status.timestamp = TimestampTimer::getTimestamp();
+    status.state     = state;
+
+    Logger::getInstance().log(status);
+}
+
+GroundModeManagerStatus GroundModeManager::getStatus()
+{
+    miosix::PauseKernelLock lock;
+    return status;
+}
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5620f6b0df2604f68b20420123b6f2e4aa196e1
--- /dev/null
+++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManager.h
@@ -0,0 +1,70 @@
+/* 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 <RIG/StateMachines/GroundModeManager/GroundModeManagerData.h>
+#include <events/EventBroker.h>
+#include <events/FSM.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+class GroundModeManager : public Boardcore::Module,
+                          public Boardcore::FSM<GroundModeManager>
+{
+public:
+    GroundModeManager();
+
+    // 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_igniting(const Boardcore::Event& event);
+
+    /**
+     * @brief Sets the ignition time
+     *
+     * @param ignitionTime Time in [ms]
+     */
+    void setIgnitionTime(uint32_t ignitionTime);
+
+    // Status getter
+    GroundModeManagerStatus getStatus();
+
+private:
+    /**
+     * @brief Logs the current FSM status on SD
+     */
+    void logStatus(GroundModeManagerState state);
+
+    // User Radio set ignition time
+    uint32_t ignitionTime;
+
+    // Current status
+    GroundModeManagerStatus status;
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("GroundModeManager");
+};
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7178a808fd103f7967688dcb25794d6400a437b
--- /dev/null
+++ b/src/boards/RIG/StateMachines/GroundModeManager/GroundModeManagerData.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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace RIG
+{
+
+enum class GroundModeManagerState : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    READY,
+    ARMED,
+    IGNITING
+};
+
+struct GroundModeManagerStatus
+{
+    long long timestamp          = 0;
+    GroundModeManagerState state = GroundModeManagerState::UNINIT;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+
+}  // namespace RIG
diff --git a/src/boards/RIG/StateMachines/TARS1/MedianFilter.h b/src/boards/RIG/StateMachines/TARS1/MedianFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7cbd259cc404f026618340f072e8085b990a67d
--- /dev/null
+++ b/src/boards/RIG/StateMachines/TARS1/MedianFilter.h
@@ -0,0 +1,202 @@
+/* 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.
+ */
+#pragma once
+
+#include <iostream>
+
+namespace RIG
+{
+/**
+ * @brief Median filter for embedded adapted to avoid recursion.
+ * The object calculates a static 1D median filter, which depends on the
+ * template parameters.
+ *
+ * @tparam T type of object to sort and median filter.
+ * @tparam S number of samples needed for a static analysis.
+ */
+template <typename T, int S>
+class MedianFilter
+{
+public:
+    MedianFilter() : m_idx(0), ready(false), m_med(0)
+    {
+        // Initialize the arrays
+        for (int i = 0; i < S; i++)
+        {
+            m_buf[i] = 0;
+            m_tmp[i] = 0;
+        }
+    }
+
+    /**
+     * @brief Adds the sample S to the window and computes the
+     * median if enough samples have been gathered.
+     *
+     * @param s The sample to add to the median filter
+     */
+    void add(T s)
+    {
+        m_buf[m_idx] = s;
+        m_idx        = (m_idx + 1) % S;
+
+        if (m_idx == 0)
+        {
+            calcMedian();
+            ready = true;
+        }
+        else
+        {
+            ready = false;
+        }
+    }
+
+    /**
+     * @brief returns the median computed when the last sample was
+     * added. Does not return anything meaningful if not enough samples
+     * have been gathered; check isReady() first.
+     *
+     * @return T the median element
+     */
+    T getMedian() { return m_med; }
+
+    /**
+     * @brief returns the state of the computed median.
+     * If the number of elements is not a multiple of the size,
+     * then ready is false and the median does not have sense with
+     * respect to the last inserted values.
+     *
+     * @note depending on the history of insertions, even if it is not ready,
+     * the median is consistent. In case of not previously inserted values it is
+     * 0 and in case of previously inserted values, the median is the previous
+     * one.
+     *
+     * @return true
+     * @return false
+     */
+    bool isReady() { return ready; }
+
+    /**
+     * @brief Resets the internal state of the object.
+     */
+    void reset()
+    {
+        m_idx = 0;
+        m_med = 0;
+        ready = false;
+
+        // Reset the arrays
+        for (int i = 0; i < S; i++)
+        {
+            m_buf[i] = 0;
+            m_tmp[i] = 0;
+        }
+    }
+
+private:
+    int m_idx;
+    bool ready;
+    T m_buf[S], m_tmp[S], m_med;
+
+    /**
+     * @brief helper to calculate the median. Copies
+     * the buffer into the temp area, then calls Hoare's in-place
+     * selection algorithm to obtain the median.
+     */
+    void calcMedian()
+    {
+        for (int i = 0; i < S; i++)
+        {
+            m_tmp[i] = m_buf[i];
+        }
+        m_med = select(0, S - 1, S / 2);
+    }
+
+    /**
+     * @brief partition function, like from quicksort.
+     * l and r are the left and right bounds of the array (m_tmp),
+     * respectively, and p is the pivot index.
+     *
+     * @param l left bound of m_tmp array
+     * @param r right bound of m_tmp array
+     * @param p pivot index
+     */
+    int partition(int l, int r, int p)
+    {
+        T tmp;
+        T pv     = m_tmp[p];
+        m_tmp[p] = m_tmp[r];
+        m_tmp[r] = pv;
+        int s    = l;
+        for (int i = l; i < r; i++)
+        {
+            if (m_tmp[i] < pv)
+            {
+                tmp      = m_tmp[s];
+                m_tmp[s] = m_tmp[i];
+                m_tmp[i] = tmp;
+                s++;
+            }
+        }
+        tmp      = m_tmp[s];
+        m_tmp[s] = m_tmp[r];
+        m_tmp[r] = tmp;
+        return s;
+    }
+
+    /**
+     * @brief Hoare's quickselect. l and r are the
+     * array bounds, and k conveys that we want to return
+     * the k-th value
+     *
+     * @param l left bound of m_tmp array
+     * @param r right bound of m_tmp array
+     * @param k wanted position of return value
+     */
+    T select(int l, int r, int k)
+    {
+        int p = 0;
+        do
+        {
+            if (l == r)
+            {
+                return m_tmp[l];
+            }
+
+            p = (l + r) / 2;
+            p = partition(l, r, p);
+
+            // The case in which k == p is evaluated outside the loop
+            if (k < p)
+            {
+                r = p - 1;
+            }
+            else if (k > p)
+            {
+                l = p + 1;
+            }
+
+        } while (p != k);
+
+        return m_tmp[k];
+    }
+};
+}  // namespace RIG
diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1.cpp b/src/boards/RIG/StateMachines/TARS1/TARS1.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..83cad4fd56eb57ce5b2ef4835be3c15fd15ba1e7
--- /dev/null
+++ b/src/boards/RIG/StateMachines/TARS1/TARS1.cpp
@@ -0,0 +1,321 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/Sensors/Sensors.h>
+#include <RIG/StateMachines/TARS1/TARS1.h>
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace Common;
+
+namespace RIG
+{
+TARS1::TARS1(TaskScheduler* sched) : FSM(&TARS1::state_idle), scheduler(sched)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_TARS);
+    EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+}
+
+bool TARS1::start()
+{
+    // Add task to the task scheduler to sample and compute the average of the
+    // sensors
+    uint8_t result = scheduler->addTask([=]() { sample(); },
+                                        Config::TARS::TARS_UPDATE_PERIOD,
+                                        TaskScheduler::Policy::RECOVER);
+
+    return ActiveObject::start() && result != 0;
+}
+
+void TARS1::state_idle(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            return logStatus(TARS1State::IDLE);
+        }
+        case FMM_INIT_OK:
+        {
+            return transition(&TARS1::state_ready);
+        }
+    }
+}
+
+void TARS1::state_ready(const Event& event)
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Reset all the stats and samples
+            {
+                Lock<FastMutex> lock(mutex);
+                samples.mass     = 0;
+                samples.pressure = 0;
+
+                massAtVenting         = 0;
+                previousMassAtVenting = 0;
+                previousPressure      = 0;
+
+                massStableCounter = 0;
+                sampleCounter     = 0;
+
+                filteredData.mass.reset();
+                filteredData.pressure.reset();
+            }
+            return logStatus(TARS1State::READY);
+        }
+        case MOTOR_START_TARS:
+        {
+            // By default close all valves
+            modules.get<Actuators>()->closeAllServo();
+            return transition(&TARS1::state_washing);
+        }
+    }
+}
+
+void TARS1::state_washing(const Event& event)
+{
+    // A static variable is not really necessary
+    static int washingTimeout = -1;
+
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            // Open the feed and venting valves for washing time
+            modules.get<Actuators>()->openServoAtomic(
+                ServosList::VENTING_VALVE,
+                Config::TARS::TARS_WASHING_OPENING_TIME +
+                    Config::TARS::TARS_WASHING_TIME_DELAY);
+
+            // Sleep to avoid actuators turn at the same time
+            Thread::sleep(2 * Config::TARS::TARS_WASHING_TIME_DELAY);
+
+            modules.get<Actuators>()->openServoAtomic(
+                ServosList::FILLING_VALVE,
+                Config::TARS::TARS_WASHING_OPENING_TIME);
+
+            // After double the washing time it translates to the refueling
+            washingTimeout = EventBroker::getInstance().postDelayed(
+                TARS_WASHING_DONE, TOPIC_TARS,
+                Config::TARS::TARS_WASHING_OPENING_TIME * 2);
+
+            return logStatus(TARS1State::WASHING);
+        }
+        case TARS_WASHING_DONE:
+        {
+            // Open the filling valve for a long time
+            modules.get<Actuators>()->openServoAtomic(
+                ServosList::FILLING_VALVE,
+                Config::TARS::TARS_FILLING_OPENING_TIME);
+
+            // Wait for PRESSURE STABILIZE WAIT TIME
+            Thread::sleep(Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME);
+
+            return transition(&TARS1::state_refueling);
+        }
+        case MOTOR_MANUAL_ACTION:
+        {
+            // Delete the post delayed event
+            EventBroker::getInstance().removeDelayed(washingTimeout);
+            return transition(&TARS1::state_ready);
+        }
+        case TMTC_ARM:
+        case MOTOR_STOP_TARS:
+        case MOTOR_IGNITION:
+        case MOTOR_START_TARS:
+        {
+            // Close all the valves
+            modules.get<Actuators>()->closeAllServo();
+            // Delete the post delayed event
+            EventBroker::getInstance().removeDelayed(washingTimeout);
+            // If another type of event is thrown the FSM returns to ready state
+            return transition(&TARS1::state_ready);
+        }
+    }
+}
+
+void TARS1::state_refueling(const Event& event)
+{
+    // A static variable is not really necessary
+    static int pressureStabilize = -1;
+
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (event)
+    {
+        case TARS_PRESSURE_STABILIZED:
+        case EV_ENTRY:
+        {
+            // Check if the algorithm can fill more
+            if (isMaximumMass())
+            {
+                if (massStableCounter >=
+                    Config::TARS::TARS_NUMBER_MASS_STABLE_ITERATIONS)
+                {
+                    massStableCounter = 0;
+                    return EventBroker::getInstance().post(TARS_FILLING_DONE,
+                                                           TOPIC_TARS);
+                }
+                else
+                {
+                    massStableCounter++;
+                }
+            }
+            else
+            {
+                massStableCounter = 0;
+            }
+
+            // Open the venting and wait for the pressure to stabilize
+            modules.get<Actuators>()->toggleServo(ServosList::VENTING_VALVE);
+            pressureStabilize = EventBroker::getInstance().postDelayed(
+                TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
+                modules.get<Actuators>()->getTiming(ServosList::VENTING_VALVE) +
+                    Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME * 5);
+
+            return logStatus(TARS1State::REFUELING);
+        }
+        case TARS_CHECK_PRESSURE_STABILIZE:
+        {
+            if (!isPressureStable())
+            {
+                // Iterate until the pressure is stable
+                pressureStabilize = EventBroker::getInstance().postDelayed(
+                    TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
+                    Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME);
+
+                // Update the previous pressure sampling
+                {
+                    Lock<FastMutex> lock(mutex);
+                    previousPressure = samples.pressure;
+                }
+
+                return;
+            }
+
+            Thread::sleep(Config::TARS::TARS_PRESSURE_STABILIZE_WAIT_TIME);
+
+            // Update mass at venting
+            {
+                Lock<FastMutex> lock(mutex);
+                previousMassAtVenting = massAtVenting;
+                massAtVenting         = samples.mass;
+            }
+
+            return EventBroker::getInstance().post(TARS_PRESSURE_STABILIZED,
+                                                   TOPIC_TARS);
+        }
+        case TARS_FILLING_DONE:
+        {
+            EventBroker::getInstance().removeDelayed(pressureStabilize);
+            // Close all the valves and stop the algorithm
+            modules.get<Actuators>()->closeAllServo();
+
+            return transition(&TARS1::state_ready);
+        }
+        case MOTOR_MANUAL_ACTION:
+        {
+            EventBroker::getInstance().removeDelayed(pressureStabilize);
+            return transition(&TARS1::state_ready);
+        }
+        case TMTC_ARM:
+        case MOTOR_STOP_TARS:
+        case MOTOR_IGNITION:
+        case MOTOR_START_TARS:
+        {
+            EventBroker::getInstance().removeDelayed(pressureStabilize);
+            // Close all the valves
+            modules.get<Actuators>()->closeAllServo();
+            // If another type of event is thrown the FSM returns to ready state
+            return transition(&TARS1::state_ready);
+        }
+    }
+}
+
+void TARS1::logStatus(TARS1State state)
+{
+    // Set the current state to be logged by the sample thread
+    status.state = state;
+}
+
+TARS1Status TARS1::getStatus()
+{
+    PauseKernelLock lock;
+    return status;
+}
+
+void TARS1::sample()
+{
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Sample the sensors and update the averages
+    {
+        Lock<FastMutex> lock(mutex);
+
+        float pressure = modules.get<Sensors>()->getTankBottomPress().pressure;
+        float mass     = modules.get<Sensors>()->getTankWeight().load;
+
+        filteredData.pressure.add(pressure);
+        filteredData.mass.add(mass);
+
+        sampleCounter++;
+
+        // When the number of samples reaches the template buffer number
+        if (filteredData.pressure.isReady())
+        {
+            samples.pressure = filteredData.pressure.getMedian();
+            samples.mass     = filteredData.mass.getMedian();
+            sampleCounter    = 0;
+
+            // Log the TARS state
+            status.timestamp = TimestampTimer::getTimestamp();
+            status.pressure  = samples.pressure;
+            status.mass      = samples.mass;
+
+            Logger::getInstance().log(status);
+        }
+    }
+}
+
+bool TARS1::isMaximumMass()
+{
+    {
+        Lock<FastMutex> lock(mutex);
+        return (abs(massAtVenting - previousMassAtVenting) <
+                Config::TARS::TARS_MASS_TOLERANCE);
+    }
+}
+
+bool TARS1::isPressureStable()
+{
+    {
+        Lock<FastMutex> lock(mutex);
+        return (abs(samples.pressure - previousPressure) <
+                Config::TARS::TARS_PRESSURE_TOLERANCE);
+    }
+}
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1.h b/src/boards/RIG/StateMachines/TARS1/TARS1.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fa5d1d89887c817117bf1897ea496461f6f896a
--- /dev/null
+++ b/src/boards/RIG/StateMachines/TARS1/TARS1.h
@@ -0,0 +1,119 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <RIG/Configs/TARSConfig.h>
+#include <RIG/StateMachines/TARS1/MedianFilter.h>
+#include <RIG/StateMachines/TARS1/TARS1Data.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <events/EventBroker.h>
+#include <events/FSM.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/Stats/Stats.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+class TARS1 : public Boardcore::Module, public Boardcore::FSM<TARS1>
+{
+public:
+    TARS1(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Overrides Active Object's method to add also the task to the task
+     * scheduler to sample the sensors and filter them
+     */
+    [[nodiscard]] bool start() override;
+
+    // FSM states
+    void state_idle(const Boardcore::Event& event);
+    void state_ready(const Boardcore::Event& event);
+    void state_washing(const Boardcore::Event& event);
+    void state_refueling(const Boardcore::Event& event);
+
+    // Status getter
+    TARS1Status getStatus();
+
+private:
+    /**
+     * @brief Logs the current FSM status on SD
+     */
+    void logStatus(TARS1State state);
+
+    /**
+     * @brief Samples the sensors and computes an average
+     */
+    void sample();
+
+    /**
+     * @brief Evaluates if the algorithm reached its maximum capabilities in
+     * filling the tank
+     */
+    bool isMaximumMass();
+
+    /**
+     * @brief Evaluates if the pressure inside the tank is at equilibrium
+     */
+    bool isPressureStable();
+
+    // Data structure output of sampling
+    struct
+    {
+        float pressure = 0;
+        float mass     = 0;
+    } samples;
+
+    // Data structure for filtering purposes
+    struct
+    {
+        MedianFilter<float, Config::TARS::TARS_FILTER_SAMPLE_NUMBER> pressure;
+        MedianFilter<float, Config::TARS::TARS_FILTER_SAMPLE_NUMBER> mass;
+    } filteredData;
+
+    // Registered mass after venting
+    float massAtVenting         = 0;
+    float previousMassAtVenting = 0;
+
+    // Registered pressure before checking
+    float previousPressure = 0;
+
+    // Number of times the mass was stable
+    uint16_t massStableCounter = 0;
+
+    // Counts the number of samples done for the average measure
+    uint16_t sampleCounter = 0;
+
+    // Current status
+    TARS1Status status;
+
+    // Dedicated scheduler for sampling sensors periodically
+    Boardcore::TaskScheduler* scheduler = nullptr;
+
+    // Mutex to synchronize sampling and reading
+    miosix::FastMutex mutex;
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("TARS1");
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StateMachines/TARS1/TARS1Data.h b/src/boards/RIG/StateMachines/TARS1/TARS1Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ce3da6852614ddc417383dbd7bb4df9a25112d6
--- /dev/null
+++ b/src/boards/RIG/StateMachines/TARS1/TARS1Data.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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace RIG
+{
+enum class TARS1State : uint8_t
+{
+    UNINIT = 0,
+    IDLE,
+    READY,
+    WASHING,
+    REFUELING,
+};
+
+struct TARS1Status
+{
+    long long timestamp = 0;
+    float pressure      = 0;
+    float mass          = 0;
+    TARS1State state    = TARS1State::UNINIT;
+
+    static std::string header() { return "timestamp,state,pressure,mass\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "," << pressure << "," << mass
+           << "\n";
+    }
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StatesMonitor/StatesMonitor.cpp b/src/boards/RIG/StatesMonitor/StatesMonitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65144b1ac369098e0c8d75758fb016874c791173
--- /dev/null
+++ b/src/boards/RIG/StatesMonitor/StatesMonitor.cpp
@@ -0,0 +1,89 @@
+/* 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 <RIG/StatesMonitor/StatesMonitor.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+namespace RIG
+{
+StatesMonitor::StatesMonitor(TaskScheduler* sched) : scheduler(sched)
+{
+    // Init the arrays
+    for (int i = 0; i < Config::StatesMonitor::BOARDS_NUMBER; i++)
+    {
+        updateTimestamps[i] = 0;
+        boardStatuses[i]    = 0;
+    }
+}
+
+bool StatesMonitor::start()
+{
+    // Add the task to the scheduler
+    uint8_t result = scheduler->addTask([=]() { this->update(); },
+                                        Config::StatesMonitor::UPDATE_PERIOD);
+    return result != 0;
+}
+
+void StatesMonitor::update()
+{
+    Lock<FastMutex> lock(mutex);
+
+    for (int i = 0; i < Config::StatesMonitor::BOARDS_NUMBER; i++)
+    {
+        // Check if the time since the last config expires
+        if (getTick() >
+            updateTimestamps[i] + Config::StatesMonitor::MAX_TIMEOUT)
+        {
+            boardStatuses[i] = 0;
+        }
+    }
+}
+
+void StatesMonitor::setBoardStatus(Common::CanConfig::Board board,
+                                   uint8_t status)
+{
+    Lock<FastMutex> lock(mutex);
+    uint8_t index = static_cast<uint8_t>(board);
+
+    // Check the validity of the board index
+    if (index < Config::StatesMonitor::BOARDS_NUMBER)
+    {
+        boardStatuses[index]    = status;
+        updateTimestamps[index] = getTick();
+    }
+}
+
+uint8_t StatesMonitor::getStatus(Common::CanConfig::Board board)
+{
+    Lock<FastMutex> lock(mutex);
+    uint8_t index = static_cast<uint8_t>(board);
+
+    if (index < Config::StatesMonitor::BOARDS_NUMBER)
+    {
+        return boardStatuses[index];
+    }
+    return 0;
+}
+
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/StatesMonitor/StatesMonitor.h b/src/boards/RIG/StatesMonitor/StatesMonitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f4d62f68f4104226dd6e0ab9d3229b76ffb4962
--- /dev/null
+++ b/src/boards/RIG/StatesMonitor/StatesMonitor.h
@@ -0,0 +1,76 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <RIG/Configs/StatesMonitorConfig.h>
+#include <common/CanConfig.h>
+#include <scheduler/TaskScheduler.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+/**
+ * @brief The class monitors and updates the states of every board.
+ * The CAN module when receives a state update, sets the current state into this
+ * module. Every N seconds, the module checks if the states have been updated,
+ * and if not resets them.
+ */
+class StatesMonitor : public Boardcore::Module
+{
+public:
+    StatesMonitor(Boardcore::TaskScheduler* sched);
+
+    /**
+     * @brief Starts the update function
+     */
+    bool start();
+
+    /**
+     * @brief Update function which checks periodically the last time in which
+     * the single states where updated. Eventually if the last update is greater
+     * than N seconds ago the state is set to 0.
+     */
+    void update();
+
+    /**
+     * @brief Sets the passed Board to a specific status
+     */
+    void setBoardStatus(Common::CanConfig::Board board, uint8_t status);
+
+    /**
+     * @brief Get the status of a specific Board
+     */
+    uint8_t getStatus(Common::CanConfig::Board board);
+
+private:
+    // Board info
+    long long int updateTimestamps[Config::StatesMonitor::BOARDS_NUMBER];
+    uint8_t boardStatuses[Config::StatesMonitor::BOARDS_NUMBER];
+
+    // Sync mutex
+    miosix::FastMutex mutex;
+
+    Boardcore::TaskScheduler* scheduler = nullptr;
+};
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/TMRepository/TMRepository.cpp b/src/boards/RIG/TMRepository/TMRepository.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58ecc23fbfb29539e09f46acdf00ed34d8cd1e71
--- /dev/null
+++ b/src/boards/RIG/TMRepository/TMRepository.cpp
@@ -0,0 +1,382 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/BoardScheduler.h>
+#include <RIG/Radio/Radio.h>
+#include <RIG/Sensors/Sensors.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIG/StateMachines/TARS1/TARS1.h>
+#include <RIG/StatesMonitor/StatesMonitor.h>
+#include <RIG/TMRepository/TMRepository.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace Boardcore;
+using namespace miosix;
+using namespace Common;
+
+namespace RIG
+{
+mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
+                                             uint8_t seq)
+{
+    mavlink_message_t msg;
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Prevents preemption, THIS FUNCTION MUST NOT yeld OR USE KERNEL. THE ONLY
+    // ALLOWED OPERATION ARE COPIES OF DATA STRUCTURES.
+    PauseKernelLock lock;
+
+    switch (tmId)
+    {
+        case SystemTMList::MAV_SYS_ID:
+        {
+            mavlink_sys_tm_t tm;
+
+            tm.timestamp       = TimestampTimer::getTimestamp();
+            tm.logger          = Logger::getInstance().isStarted();
+            tm.event_broker    = EventBroker::getInstance().isRunning();
+            tm.radio           = modules.get<Radio>()->isStarted();
+            tm.sensors         = modules.get<Sensors>()->isStarted();
+            tm.board_scheduler = modules.get<BoardScheduler>()->isStarted();
+
+            mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                      Config::Radio::MAV_COMPONENT_ID, &msg,
+                                      &tm);
+            break;
+        }
+        case SystemTMList::MAV_LOGGER_ID:
+        {
+            mavlink_logger_tm_t tm;
+
+            LoggerStats stats = Logger::getInstance().getStats();
+
+            tm.timestamp          = TimestampTimer::getTimestamp();
+            tm.log_number         = stats.logNumber;
+            tm.too_large_samples  = stats.tooLargeSamples;
+            tm.dropped_samples    = stats.droppedSamples;
+            tm.queued_samples     = stats.queuedSamples;
+            tm.buffers_filled     = stats.buffersFilled;
+            tm.buffers_written    = stats.buffersWritten;
+            tm.writes_failed      = stats.writesFailed;
+            tm.last_write_error   = stats.lastWriteError;
+            tm.average_write_time = stats.averageWriteTime;
+            tm.max_write_time     = stats.maxWriteTime;
+
+            mavlink_msg_logger_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                         Config::Radio::MAV_COMPONENT_ID, &msg,
+                                         &tm);
+
+            break;
+        }
+        case SystemTMList::MAV_MAVLINK_STATS:
+        {
+            mavlink_mavlink_stats_tm_t tm;
+
+            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(Config::Radio::MAV_SYSTEM_ID,
+                                                Config::Radio::MAV_COMPONENT_ID,
+                                                &msg, &tm);
+            break;
+        }
+        case SystemTMList::MAV_GSE_ID:
+        {
+            mavlink_gse_tm_t tm;
+
+            HX711Data tank   = modules.get<Sensors>()->getTankWeight();
+            HX711Data vessel = modules.get<Sensors>()->getVesselWeight();
+
+            tm.timestamp       = TimestampTimer::getTimestamp();
+            tm.loadcell_rocket = tank.load;
+            tm.loadcell_vessel = vessel.load;
+            tm.filling_pressure =
+                modules.get<Sensors>()->getFillingPress().pressure;
+            tm.vessel_pressure =
+                modules.get<Sensors>()->getVesselPress().pressure;
+            // TODO change the threshold (cannot be 0 due to angle tolerance)
+            tm.filling_valve_state = modules.get<Actuators>()->getServoPosition(
+                                         ServosList::FILLING_VALVE) > 0.3
+                                         ? 1
+                                         : 0;
+            tm.venting_valve_state = modules.get<Actuators>()->getServoPosition(
+                                         ServosList::VENTING_VALVE) > 0.3
+                                         ? 1
+                                         : 0;
+            tm.release_valve_state = modules.get<Actuators>()->getServoPosition(
+                                         ServosList::RELEASE_VALVE) > 0.3
+                                         ? 1
+                                         : 0;
+
+            tm.main_valve_state = modules.get<Actuators>()->getServoPosition(
+                                      ServosList::MAIN_VALVE) > 0.3
+                                      ? 1
+                                      : 0;
+
+            tm.arming_state =
+                modules.get<GroundModeManager>()->getStatus().state ==
+                        GroundModeManagerState::ARMED
+                    ? 1
+                    : 0;
+            tm.ignition_state =
+                modules.get<GroundModeManager>()->getStatus().state ==
+                        GroundModeManagerState::IGNITING
+                    ? 1
+                    : 0;
+            tm.tars_state =
+                static_cast<uint8_t>(modules.get<TARS1>()->getStatus().state);
+            tm.battery_voltage =
+                modules.get<Sensors>()->getBatteryVoltage().voltage;
+            tm.current_consumption =
+                modules.get<Sensors>()->getCurrent().current;
+
+            // Board statuses
+            tm.main_board_status =
+                modules.get<StatesMonitor>()->getStatus(CanConfig::Board::MAIN);
+            tm.payload_board_status = modules.get<StatesMonitor>()->getStatus(
+                CanConfig::Board::PAYLOAD);
+            tm.motor_board_status = modules.get<StatesMonitor>()->getStatus(
+                CanConfig::Board::MOTOR);
+
+            mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                      Config::Radio::MAV_COMPONENT_ID, &msg,
+                                      &tm);
+            break;
+        }
+        case SystemTMList::MAV_MOTOR_ID:
+        {
+            mavlink_motor_tm_t tm;
+
+            TemperatureData temp =
+                modules.get<Sensors>()->getThermocoupleLastSample();
+
+            tm.timestamp = TimestampTimer::getTimestamp();
+            tm.top_tank_pressure =
+                modules.get<Sensors>()->getTankTopPress().pressure;
+            tm.bottom_tank_pressure =
+                modules.get<Sensors>()->getTankBottomPress().pressure;
+            tm.combustion_chamber_pressure =
+                modules.get<Sensors>()->getCCPress().pressure;
+            tm.tank_temperature = temp.temperature;
+            tm.floating_level   = 0;
+
+            tm.battery_voltage =
+                modules.get<Sensors>()->getMotorBatteryVoltage().batVoltage;
+            tm.current_consumption =
+                modules.get<Sensors>()->getMotorCurrent().current;
+
+            tm.main_valve_state = modules.get<Actuators>()->getServoPosition(
+                                      ServosList::MAIN_VALVE) > 0.3
+                                      ? 1
+                                      : 0;
+
+            mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                        Config::Radio::MAV_COMPONENT_ID, &msg,
+                                        &tm);
+
+            break;
+        }
+        default:
+        {
+            // No system ID recognized, send a nack
+            mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                                     Config::Radio::MAV_COMPONENT_ID, &msg,
+                                     msgId, seq);
+            break;
+        }
+    }
+    return msg;
+}
+
+mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
+                                              uint8_t msgId, uint8_t seq)
+{
+    mavlink_message_t msg;
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // NO NEED FOR KERNEL LOCK BECAUSE IT IS ALREADY PRESENT INSIDE THE GETTERS
+    switch (sensorId)
+    {
+        case SensorsTMList::MAV_FILLING_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData sample = modules.get<Sensors>()->getFillingPress();
+
+            tm.timestamp = sample.pressureTimestamp;
+            tm.pressure  = sample.pressure;
+
+            mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                           Config::Radio::MAV_COMPONENT_ID,
+                                           &msg, &tm);
+
+            break;
+        }
+        case SensorsTMList::MAV_TANK_TOP_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData sample = modules.get<Sensors>()->getTankTopPress();
+
+            tm.timestamp = sample.pressureTimestamp;
+            tm.pressure  = sample.pressure;
+
+            mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                           Config::Radio::MAV_COMPONENT_ID,
+                                           &msg, &tm);
+
+            break;
+        }
+        case SensorsTMList::MAV_TANK_BOTTOM_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData sample = modules.get<Sensors>()->getTankBottomPress();
+
+            tm.timestamp = sample.pressureTimestamp;
+            tm.pressure  = sample.pressure;
+
+            mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                           Config::Radio::MAV_COMPONENT_ID,
+                                           &msg, &tm);
+
+            break;
+        }
+        case SensorsTMList::MAV_VESSEL_PRESS_ID:
+        {
+            mavlink_pressure_tm_t tm;
+
+            PressureData sample = modules.get<Sensors>()->getVesselPress();
+
+            tm.timestamp = sample.pressureTimestamp;
+            tm.pressure  = sample.pressure;
+
+            mavlink_msg_pressure_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                           Config::Radio::MAV_COMPONENT_ID,
+                                           &msg, &tm);
+
+            break;
+        }
+        case SensorsTMList::MAV_TANK_TEMP_ID:
+        {
+            mavlink_temp_tm_t tm;
+
+            TemperatureData temp =
+                modules.get<Sensors>()->getThermocoupleLastSample();
+
+            tm.timestamp   = temp.temperatureTimestamp;
+            tm.temperature = temp.temperature;
+
+            mavlink_msg_temp_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                       Config::Radio::MAV_COMPONENT_ID, &msg,
+                                       &tm);
+
+            break;
+        }
+        case SensorsTMList::MAV_LOAD_CELL_VESSEL_ID:
+        {
+            mavlink_load_tm_t tm;
+
+            HX711Data load = modules.get<Sensors>()->getVesselWeight();
+
+            tm.timestamp = load.loadTimestamp;
+            tm.load      = load.load;
+
+            mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                       Config::Radio::MAV_COMPONENT_ID, &msg,
+                                       &tm);
+            break;
+        }
+        case SensorsTMList::MAV_LOAD_CELL_TANK_ID:
+        {
+            mavlink_load_tm_t tm;
+
+            HX711Data load = modules.get<Sensors>()->getTankWeight();
+
+            tm.timestamp = load.loadTimestamp;
+            tm.load      = load.load;
+
+            mavlink_msg_load_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                       Config::Radio::MAV_COMPONENT_ID, &msg,
+                                       &tm);
+            break;
+        }
+        default:
+        {
+            // No sensor ID recognized, send a nack
+            mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                                     Config::Radio::MAV_COMPONENT_ID, &msg,
+                                     msgId, seq);
+            break;
+        }
+    }
+    return msg;
+}
+
+mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId,
+                                            uint8_t seq)
+{
+    mavlink_message_t msg;
+    ModuleManager& modules = ModuleManager::getInstance();
+
+    // Prevents preemption, THIS FUNCTION MUST NOT yeld OR USE KERNEL. THE ONLY
+    // ALLOWED OPERATION ARE COPIES OF DATA STRUCTURES.
+    PauseKernelLock lock;
+
+    if (servoId != MAIN_VALVE && servoId != VENTING_VALVE &&
+        servoId != RELEASE_VALVE && servoId != FILLING_VALVE &&
+        servoId != DISCONNECT_SERVO)
+    {
+        // No servo ID recognized, send a nack
+        mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
+                                 Config::Radio::MAV_COMPONENT_ID, &msg, msgId,
+                                 seq);
+    }
+    else
+    {
+        mavlink_servo_tm_t tm;
+
+        tm.servo_id       = servoId;
+        tm.servo_position = modules.get<Actuators>()->getServoPosition(servoId);
+
+        mavlink_msg_servo_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                    Config::Radio::MAV_COMPONENT_ID, &msg, &tm);
+    }
+
+    return msg;
+}
+}  // namespace RIG
\ No newline at end of file
diff --git a/src/boards/RIG/TMRepository/TMRepository.h b/src/boards/RIG/TMRepository/TMRepository.h
new file mode 100644
index 0000000000000000000000000000000000000000..c943398d4b5a884fd8a5e22f147ee711d8ae3fcb
--- /dev/null
+++ b/src/boards/RIG/TMRepository/TMRepository.h
@@ -0,0 +1,53 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <common/Mavlink.h>
+#include <diagnostic/PrintLogger.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIG
+{
+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 RIG
\ No newline at end of file
diff --git a/src/entrypoints/RIG/rig-entry.cpp b/src/entrypoints/RIG/rig-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00eb41ad707d7ae823b609671ed5f9571dfcac88
--- /dev/null
+++ b/src/entrypoints/RIG/rig-entry.cpp
@@ -0,0 +1,233 @@
+/* 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 <RIG/Actuators/Actuators.h>
+#include <RIG/BoardScheduler.h>
+#include <RIG/Buses.h>
+#include <RIG/CanHandler/CanHandler.h>
+#include <RIG/Radio/Radio.h>
+#include <RIG/Sensors/Sensors.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIG/StateMachines/TARS1/TARS1.h>
+#include <RIG/StatesMonitor/StatesMonitor.h>
+#include <RIG/TMRepository/TMRepository.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/EventBroker.h>
+#include <events/EventData.h>
+#include <events/utils/EventSniffer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+using namespace Boardcore;
+using namespace RIG;
+using namespace miosix;
+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
+    Buses* buses              = new Buses();
+    BoardScheduler* scheduler = new BoardScheduler();
+    Actuators* actuators      = new Actuators(scheduler->getScheduler(4));
+    Sensors* sensors          = new Sensors(scheduler->getScheduler(3));
+    Radio* radio              = new Radio();
+    TMRepository* tmRepo      = new TMRepository();
+    GroundModeManager* groundModeManager = new GroundModeManager();
+    TARS1* tars = new TARS1(scheduler->getScheduler(4));
+    CanHandler* can =
+        new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
+    StatesMonitor* states =
+        new StatesMonitor(scheduler->getScheduler(miosix::MAIN_PRIORITY));
+
+    // Initialize singleton modules
+    Logger& SDlogger    = Logger::getInstance();
+    EventBroker& broker = EventBroker::getInstance();
+
+    // Insert modules
+    if (!modules.insert<Buses>(buses))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the buses module");
+    }
+
+    if (!modules.insert<BoardScheduler>(scheduler))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the scheduler module");
+    }
+
+    if (!modules.insert<Sensors>(sensors))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the sensors module");
+    }
+
+    if (!modules.insert<Actuators>(actuators))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the actuators module");
+    }
+
+    if (!modules.insert<Radio>(radio))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the radio module");
+    }
+
+    if (!modules.insert<TMRepository>(tmRepo))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the tm repository module");
+    }
+
+    if (!modules.insert<GroundModeManager>(groundModeManager))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the ground mode manager FSM module");
+    }
+
+    if (!modules.insert<TARS1>(tars))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the tars module");
+    }
+
+    if (!modules.insert<CanHandler>(can))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the CAN module");
+    }
+
+    if (!modules.insert<StatesMonitor>(states))
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error inserting the States Monitor module");
+    }
+
+    // Start singleton modules
+    if (!SDlogger.testSDCard())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the SD logger");
+    }
+
+    if (!broker.start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the Event Broker");
+    }
+
+    // Start modules
+    if (!modules.get<Actuators>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the actuators module");
+    }
+
+    if (!modules.get<BoardScheduler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the board scheduler");
+    }
+
+    if (!modules.get<Sensors>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the sensors module");
+    }
+
+    if (!modules.get<StatesMonitor>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the States Monitor module");
+    }
+
+    if (!modules.get<Radio>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the radio module");
+    }
+
+    if (!modules.get<GroundModeManager>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the ground mode manager module");
+    }
+
+    if (!modules.get<TARS1>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the tars module");
+    }
+
+    if (!modules.get<CanHandler>()->start())
+    {
+        initResult = false;
+        LOG_ERR(logger, "Error starting the CAN module");
+    }
+
+    if (initResult)
+    {
+        // POST OK
+        ui::redLed::low();
+        EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_MOTOR);
+        LOG_INFO(logger, "Init success!");
+    }
+    else
+    {
+        // POST ERROR
+        ui::redLed::high();
+        EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_MOTOR);
+    }
+
+    // Log all events
+    EventSniffer sniffer(
+        EventBroker::getInstance(), TOPICS_LIST,
+        [](uint8_t event, uint8_t topic)
+        {
+            EventData ev{TimestampTimer::getTimestamp(), event, topic};
+            Logger::getInstance().log(ev);
+        });
+
+    // 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
index 4fb6d768353e40f6f25ed01cf497920e8b3713b5..7f8e02f015244138af0e4acb4eeedeb9ed52282f 100644
--- a/src/scripts/EventDumper/EventDumper.cpp
+++ b/src/scripts/EventDumper/EventDumper.cpp
@@ -19,7 +19,6 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-
 #include <common/Events.h>
 
 #include <iostream>
diff --git a/src/scripts/logdecoder/RIG/Makefile b/src/scripts/logdecoder/RIG/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..6bbaf34fc2008754ca1425004f33514659c56baf
--- /dev/null
+++ b/src/scripts/logdecoder/RIG/Makefile
@@ -0,0 +1,16 @@
+BOARDCORE := ../../../../skyward-boardcore/
+OBSW := ../../../../src/boards/
+
+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)
+clean:
+	rm logdecoder
diff --git a/src/scripts/logdecoder/RIG/logdecoder.cpp b/src/scripts/logdecoder/RIG/logdecoder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d9c602b10f0dd896cbf9f861995e02c68e669b8
--- /dev/null
+++ b/src/scripts/logdecoder/RIG/logdecoder.cpp
@@ -0,0 +1,134 @@
+/* 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 <RIG/Actuators/ActuatorsData.h>
+#include <RIG/CanHandler/CanHandlerData.h>
+#include <RIG/Sensors/ADCsData.h>
+#include <RIG/Sensors/LoadCellsData.h>
+#include <RIG/StateMachines/GroundModeManager/GroundModeManagerData.h>
+#include <RIG/StateMachines/TARS1/TARS1Data.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 RIG;
+
+void registerTypes(Deserializer& ds)
+{
+    // Register all Boardcore types
+    LogTypes::registerTypes(ds);
+
+    // Custom types
+    ds.registerType<GroundModeManagerStatus>();
+    ds.registerType<LoadCellsData>();
+    ds.registerType<ADCsData>();
+    ds.registerType<ActuatorsData>();
+    ds.registerType<TARS1Status>();
+    ds.registerType<CanCurrentSensor>();
+    ds.registerType<CanPressureSensor>();
+    ds.registerType<CanTemperatureSensor>();
+    ds.registerType<CanVoltageSensor>();
+}
+
+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;
+}