diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index f15b5c01bf51bf4a26af6e9e52d0fba696673210..2874d30e76943630ecde23ae28e94aed5c3d895b 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -85,7 +85,7 @@ set(PAYLOAD_COMPUTER
     src/boards/Payload/Sensors/Sensors.cpp
     src/boards/Payload/PinHandler/PinHandler.cpp
     src/boards/Payload/Radio/Radio.cpp
-    src/boards/Payload/TMRepository/TMRepository.cpp
+    src/boards/Payload/Radio/MessageHandler.cpp
     src/boards/Payload/StateMachines/NASController/NASController.cpp
     src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
     src/boards/Payload/StateMachines/WingController/WingController.cpp
diff --git a/src/boards/Payload/Configs/ActuatorsConfig.h b/src/boards/Payload/Configs/ActuatorsConfig.h
index 106c0dba16e35fadfa6a59393a687c766c1687e7..92c9c06a5fb3a871b20f85e3cdaa04d1bb2a524e 100644
--- a/src/boards/Payload/Configs/ActuatorsConfig.h
+++ b/src/boards/Payload/Configs/ActuatorsConfig.h
@@ -31,10 +31,7 @@ namespace Config
 namespace Actuators
 {
 
-// clang-format off
-// Indent to avoid the linter complaining about using namespace
-  using namespace std::chrono;
-// clang-format on
+/* linter off */ using namespace std::chrono_literals;
 
 namespace LeftServo
 {
diff --git a/src/boards/Payload/Configs/PinHandlerConfig.h b/src/boards/Payload/Configs/PinHandlerConfig.h
index 75d6cbde25a61d76de250725863517eeb93a062f..92f984c1c417c3d0a0886a243ac4a9c0aa057409 100644
--- a/src/boards/Payload/Configs/PinHandlerConfig.h
+++ b/src/boards/Payload/Configs/PinHandlerConfig.h
@@ -24,6 +24,8 @@
 
 #include <utils/PinObserver/PinObserver.h>
 
+#include <chrono>
+
 namespace Payload
 {
 namespace Config
@@ -31,13 +33,25 @@ namespace Config
 namespace PinHandler
 {
 
+/* linter-off */ using namespace std::chrono_literals;
+
+namespace PinObserver
+{
+constexpr auto PERIOD = 20ms;
+}
+
 namespace NoseconeDetach
 {
 constexpr auto DETECTION_THRESHOLD   = 20;
-constexpr auto PERIOD                = 20;  // [ms]
 constexpr auto TRIGGERING_TRANSITION = Boardcore::PinTransition::RISING_EDGE;
 }  // namespace NoseconeDetach
 
+namespace ExpulsionSense
+{
+constexpr auto DETECTION_THRESHOLD   = 20;
+constexpr auto TRIGGERING_TRANSITION = Boardcore::PinTransition::RISING_EDGE;
+}  // namespace ExpulsionSense
+
 }  // namespace PinHandler
 }  // namespace Config
 }  // namespace Payload
diff --git a/src/boards/Payload/Configs/RadioConfig.h b/src/boards/Payload/Configs/RadioConfig.h
index 8bbf64ff9d3684f475157b3f41209ae188be0f10..6579f707466c1500f4d839ba7ed68b3c2fb06f86 100644
--- a/src/boards/Payload/Configs/RadioConfig.h
+++ b/src/boards/Payload/Configs/RadioConfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -21,26 +21,41 @@
  */
 #pragma once
 
-#include <common/MavlinkGemini.h>
+#include <common/Mavlink.h>
+#include <units/Frequency.h>
+
+#include <chrono>
 
 namespace Payload
 {
-namespace RadioConfig
+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;
 
-constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 200;
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace std::chrono_literals;
 
-constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50;
-constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10;
+constexpr auto LOW_RATE_TELEMETRY         = 2_hz;
+constexpr auto HIGH_RATE_TELEMETRY        = 4_hz;
+constexpr auto MESSAGE_QUEUE_SIZE         = 10;
+constexpr auto MESSAGE_QUEUE_FLUSH_PERIOD = 200ms;
 
-constexpr uint8_t MAV_SYSTEM_ID = 171;
-constexpr uint8_t MAV_COMP_ID   = 96;
+namespace Mavlink
+{
+constexpr uint8_t SYSTEM_ID    = 171;
+constexpr uint8_t COMPONENT_ID = 96;
+}  // namespace Mavlink
 
-constexpr uint32_t MAVLINK_QUEUE_SIZE = 10;
+namespace MavlinkDriver
+{
+constexpr auto PKT_LENGTH       = 255;
+constexpr auto PKT_QUEUE_SIZE   = 20;
+constexpr auto MSG_LENGTH       = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+constexpr auto SLEEP_AFTER_SEND = 0ms;
+constexpr auto MAX_PKT_AGE      = 10ms;
+}  // namespace MavlinkDriver
 
-}  // namespace RadioConfig
+}  // namespace Radio
+}  // namespace Config
 }  // namespace Payload
diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h
index 08fe426a075d665045217512651af78d8e52855e..a1b315208de6e15b6ab44ac9082f80da5df1c426 100644
--- a/src/boards/Payload/Configs/SensorsConfig.h
+++ b/src/boards/Payload/Configs/SensorsConfig.h
@@ -41,11 +41,8 @@ namespace Config
 namespace Sensors
 {
 
-// clang-format off
-// Indent to avoid the linter complaining about using namespace
-  using namespace Boardcore::Units::Frequency;
-  using namespace std::chrono_literals;
-// clang-format on
+/* linter off */ using namespace Boardcore::Units::Frequency;
+/* linter off */ using namespace std::chrono_literals;
 
 namespace LPS22DF
 {
@@ -109,11 +106,6 @@ constexpr auto SAMPLING_RATE = 100_hz;
 constexpr auto OVERSAMPLING_RATIO =
     Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192;
 constexpr bool GLOBAL_CHOP_MODE = true;
-// TODO: populate with final channels once sensors are soldered
-constexpr auto STATIC_PRESSURE_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
-constexpr auto DYNAMIC_PRESSURE_CH =
-    Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
 }  // namespace ADS131M08
 
 namespace InternalADC
@@ -131,12 +123,16 @@ namespace StaticPressure
 {
 constexpr auto ENABLED       = true;
 constexpr auto SAMPLING_RATE = ADS131M08::SAMPLING_RATE;
+constexpr auto ADC_CH        = Boardcore::ADS131M08Defs::Channel::CHANNEL_0;
+constexpr auto SCALE         = (38300.0f + 13000.0f) / 13000.0f;
 }  // namespace StaticPressure
 
 namespace DynamicPressure
 {
 constexpr auto ENABLED       = true;
 constexpr auto SAMPLING_RATE = ADS131M08::SAMPLING_RATE;
+constexpr auto ADC_CH        = Boardcore::ADS131M08Defs::Channel::CHANNEL_1;
+constexpr auto SCALE         = (38300.0f + 13000.0f) / 13000.0f;
 }  // namespace DynamicPressure
 
 namespace Pitot
diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
index 3d5b103409bf03b25b3726cf621cb8e69a46afb3..ae0cba716635762a1e3660cdc0e48feda0e00b07 100644
--- a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
+++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
@@ -73,7 +73,7 @@ void FlightStatsRecorder::update()
     // Data gathering
     Sensors* sensor           = getModule<Sensors>();
     AccelerometerData accData = sensor->getIMULastSample().accData;
-    PressureData baroData     = sensor->getStaticPressureLastSample();
+    PressureData baroData     = sensor->getStaticPressure();
     PitotData pitotData       = sensor->getPitotLastSample();
     GPSData gpsData           = sensor->getUBXGPSLastSample();
     NASState nasData          = getModule<NASController>()->getNasState();
diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp
index 16ea05c5d90fa53f1c24457c7942507ed855d200..6ca5156517dc570795886895d578bbba280ce298 100644
--- a/src/boards/Payload/PinHandler/PinHandler.cpp
+++ b/src/boards/Payload/PinHandler/PinHandler.cpp
@@ -40,43 +40,67 @@ namespace Payload
 
 bool PinHandler::start()
 {
+    using namespace std::chrono;
+
     auto& scheduler = getModule<BoardScheduler>()->pinHandler();
 
-    pinObserver = std::make_unique<PinObserver>(scheduler,
-                                                config::NoseconeDetach::PERIOD);
+    pinObserver = std::make_unique<PinObserver>(
+        scheduler, milliseconds{config::PinObserver::PERIOD}.count());
 
-    bool pinResult = pinObserver->registerPinCallback(
+    bool detachPayloadResult = pinObserver->registerPinCallback(
         hwmap::detachPayload::getPin(),
-        [this](auto t) { onDetachPinTransition(t); },
+        [this](auto t) { onDetachPayloadTransition(t); },
         config::NoseconeDetach::DETECTION_THRESHOLD);
 
-    if (!pinResult)
+    if (!detachPayloadResult)
     {
+        LOG_ERR(logger,
+                "Failed to register pin callback for the detach payload pin");
         return false;
     }
 
-    started = true;
+    bool expSenseResult = pinObserver->registerPinCallback(
+        hwmap::expulsionSense::getPin(),
+        [this](auto t) { onExpulsionSenseTransition(t); },
+        config::ExpulsionSense::DETECTION_THRESHOLD);
+
+    if (!expSenseResult)
+    {
+        LOG_ERR(logger,
+                "Failed to register pin callback for the expulsion sense pin");
+        return false;
+    }
 
+    started = true;
     return true;
 }
 
 bool PinHandler::isStarted() { return started; }
 
-std::map<PinsList, PinData> PinHandler::getPinData()
+void PinHandler::onDetachPayloadTransition(PinTransition transition)
 {
-    std::map<PinsList, PinData> data;
-
-    data[PinsList::NOSECONE_PIN] =
-        pinObserver->getPinData(hwmap::detachPayload::getPin());
+    // TODO: send event
+    LOG_INFO(logger, "onDetachPayloadTransition {}",
+             static_cast<int>(transition));
+}
 
-    return data;
+void PinHandler::onExpulsionSenseTransition(PinTransition transition)
+{
+    // TODO: send event
+    LOG_INFO(logger, "onExpulsionSenseTransition {}",
+             static_cast<int>(transition));
 }
 
-void PinHandler::onDetachPinTransition(PinTransition transition)
+PinData PinHandler::getPinData(PinList pin)
 {
-    if (transition == config::NoseconeDetach::TRIGGERING_TRANSITION)
+    switch (pin)
     {
-        EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT);
+        case PinList::DETACH_PAYLOAD_PIN:
+            return pinObserver->getPinData(hwmap::detachPayload::getPin());
+        case PinList::EXPULSION_SENSE:
+            return pinObserver->getPinData(hwmap::expulsionSense::getPin());
+        default:
+            return PinData{};
     }
 }
 
diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h
index e274b17af529d659c4bbb2d8987535ccc0159f0a..8d957cc91b4c1ddc40ecce624fb269ce227d21f2 100644
--- a/src/boards/Payload/PinHandler/PinHandler.h
+++ b/src/boards/Payload/PinHandler/PinHandler.h
@@ -31,6 +31,12 @@ namespace Payload
 {
 class BoardScheduler;
 
+enum class PinList : uint8_t
+{
+    DETACH_PAYLOAD_PIN = 2,
+    EXPULSION_SENSE    = 3,
+};
+
 /**
  * @brief This class contains the handlers for the detach pins on the rocket.
  *
@@ -40,24 +46,22 @@ class BoardScheduler;
 class PinHandler : public Boardcore::InjectableWithDeps<BoardScheduler>
 {
 public:
-    bool start();
+    [[nodiscard]] bool start();
 
     bool isStarted();
 
     /**
-     * @brief Returns information about all pins handled by this class
+     * @brief Returns information about the specified pin.
      */
-    std::map<PinsList, Boardcore::PinData> getPinData();
+    Boardcore::PinData getPinData(PinList pin);
 
 private:
-    /**
-     * @brief Detach pin transition handler.
-     */
-    void onDetachPinTransition(Boardcore::PinTransition transition);
+    void onDetachPayloadTransition(Boardcore::PinTransition transition);
+    void onExpulsionSenseTransition(Boardcore::PinTransition transition);
 
     std::unique_ptr<Boardcore::PinObserver> pinObserver;
 
-    bool started = false;
+    std::atomic<bool> started{false};
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler");
 };
diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bed62957684b5a9ee4c2dc104250f6ef51d3f2a4
--- /dev/null
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -0,0 +1,652 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * 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 <Payload/Actuators/Actuators.h>
+#include <Payload/AltitudeTrigger/AltitudeTrigger.h>
+#include <Payload/BoardScheduler.h>
+#include <Payload/CanHandler/CanHandler.h>
+#include <Payload/PinHandler/PinHandler.h>
+#include <Payload/Sensors/Sensors.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <Payload/StateMachines/NASController/NASController.h>
+#include <Payload/StateMachines/WingController/WingController.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <diagnostic/CpuMeter/CpuMeter.h>
+#include <events/EventBroker.h>
+
+#include "Radio.h"
+
+using namespace Boardcore;
+using namespace Common;
+namespace config = Payload::Config::Radio;
+
+namespace Payload
+{
+
+void Radio::handleMessage(const mavlink_message_t& msg)
+{
+    switch (msg.msgid)
+    {
+
+        case MAVLINK_MSG_ID_PING_TC:
+        {
+            return enqueueAck(msg);
+        }
+
+        case MAVLINK_MSG_ID_COMMAND_TC:
+        {
+            return handleCommand(msg);
+        }
+
+        case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
+        {
+            auto tmId = static_cast<SystemTMList>(
+                mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
+
+            if (!enqueueSystemTm(tmId))
+            {
+                return enqueueNack(msg);
+            }
+
+            return enqueueAck(msg);
+        }
+
+        case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
+        {
+            auto sensorId = static_cast<SensorsTMList>(
+                mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
+
+            if (!enqueueSensorsTm(sensorId))
+            {
+                return enqueueNack(msg);
+            }
+
+            return enqueueAck(msg);
+        }
+
+        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+        {
+            bool testMode = getModule<FlightModeManager>()->getStatus().state ==
+                            FlightModeManagerState::TEST_MODE;
+
+            // Perform the wiggle in test mode only
+            if (!testMode)
+            {
+                return enqueueNack(msg);
+            }
+
+            auto servo = static_cast<ServosList>(
+                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
+
+            getModule<Actuators>()->wiggleServo(servo);
+            return enqueueAck(msg);
+        }
+
+        case MAVLINK_MSG_ID_RAW_EVENT_TC:
+        {
+            uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
+            uint8_t eventId = mavlink_msg_raw_event_tc_get_event_id(&msg);
+
+            bool testMode = getModule<FlightModeManager>()->getStatus().state ==
+                            FlightModeManagerState::TEST_MODE;
+
+            // Raw events are allowed in test mode only
+            if (!testMode)
+            {
+                return enqueueNack(msg);
+            }
+
+            EventBroker::getInstance().post(topicId, eventId);
+            return enqueueAck(msg);
+        }
+
+        default:
+        {
+            return enqueueNack(msg);
+        }
+    }
+}
+
+void Radio::handleCommand(const mavlink_message_t& msg)
+{
+    // Command-to-event look-up table, uses operator[] to retrieve an event from
+    // a command
+    static struct
+    {
+        Events operator[](MavCommandList command)
+        {
+#define MAP_CMD(command, event) \
+    case command:               \
+        return event
+
+            switch (command)
+            {
+                MAP_CMD(MAV_CMD_ARM, TMTC_ARM);
+                MAP_CMD(MAV_CMD_DISARM, TMTC_DISARM);
+                MAP_CMD(MAV_CMD_CALIBRATE, TMTC_CALIBRATE);
+                MAP_CMD(MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT);
+                MAP_CMD(MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH);
+                MAP_CMD(MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING);
+                MAP_CMD(MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION);
+                MAP_CMD(MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT);
+                MAP_CMD(MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD);
+                MAP_CMD(MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE);
+                MAP_CMD(MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE);
+                MAP_CMD(MAV_CMD_START_RECORDING, TMTC_START_RECORDING);
+                MAP_CMD(MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING);
+                default:
+                    return LAST_EVENT;
+            }
+#undef MAP_CMD
+        }
+    } commandToEvent;  // Command-to-event look-up table
+
+    auto command = static_cast<MavCommandList>(
+        mavlink_msg_command_tc_get_command_id(&msg));
+
+    switch (command)
+    {
+        case MAV_CMD_START_LOGGING:
+        {
+            bool started = Logger::getInstance().start();
+            if (!started)
+            {
+                return enqueueNack(msg);
+            }
+
+            Logger::getInstance().resetStats();
+            return enqueueAck(msg);
+        }
+
+        case MAV_CMD_STOP_LOGGING:
+        {
+            Logger::getInstance().stop();
+            return enqueueAck(msg);
+        }
+
+        case MAV_CMD_SAVE_CALIBRATION:
+        {
+            bool magResult = getModule<Sensors>()->writeMagCalibration();
+            if (magResult)
+            {
+                return enqueueAck(msg);
+            }
+            else
+            {
+                return enqueueNack(msg);
+            }
+        }
+
+        default:
+        {
+            // Map the command to an event and post it, if it exists
+            auto event = commandToEvent[command];
+
+            if (event == LAST_EVENT)
+            {
+                return enqueueNack(msg);
+            }
+
+            EventBroker::getInstance().post(event, TOPIC_TMTC);
+            return enqueueAck(msg);
+        }
+    }
+}
+
+bool Radio::enqueueSystemTm(SystemTMList tmId)
+{
+    switch (tmId)
+    {
+        case MAV_SENSORS_STATE_ID:
+        {
+            auto sensors = getModule<Sensors>()->getSensorInfo();
+            for (auto sensor : sensors)
+            {
+                mavlink_message_t msg;
+                mavlink_sensor_state_tm_t tm;
+
+                strcpy(tm.sensor_name, sensor.id.c_str());
+                tm.initialized = sensor.isInitialized ? 1 : 0;
+                tm.enabled     = sensor.isEnabled ? 1 : 0;
+
+                mavlink_msg_sensor_state_tm_encode(
+                    config::Mavlink::SYSTEM_ID, config::Mavlink::COMPONENT_ID,
+                    &msg, &tm);
+                enqueueMessage(msg);
+            }
+
+            return true;
+        }
+
+        case MAV_SYS_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_sys_tm_t tm;
+
+            tm.timestamp    = TimestampTimer::getTimestamp();
+            tm.logger       = Logger::getInstance().isStarted() ? 1 : 0;
+            tm.event_broker = EventBroker::getInstance().isRunning() ? 1 : 0;
+            tm.radio        = isStarted() ? 1 : 0;
+            tm.sensors      = getModule<Sensors>()->isStarted() ? 1 : 0;
+            tm.actuators    = 0;  // TODO: implement isStarted
+            tm.pin_handler  = getModule<PinHandler>()->isStarted() ? 1 : 0;
+            tm.can_handler  = getModule<CanHandler>()->isStarted() ? 1 : 0;
+            tm.scheduler    = getModule<BoardScheduler>()->isStarted() ? 1 : 0;
+
+            mavlink_msg_sys_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_LOGGER_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_logger_tm_t tm;
+
+            // Get the logger stats
+            LoggerStats stats = Logger::getInstance().getStats();
+
+            tm.timestamp          = stats.timestamp;
+            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::Mavlink::SYSTEM_ID,
+                                         config::Mavlink::COMPONENT_ID, &msg,
+                                         &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_MAVLINK_STATS_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_mavlink_stats_tm_t tm;
+
+            // Get the mavlink stats
+            MavlinkStatus stats = 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::Mavlink::SYSTEM_ID,
+                                                config::Mavlink::COMPONENT_ID,
+                                                &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_FLIGHT_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_payload_flight_tm_t tm;
+
+            Sensors* sensors       = getModule<Sensors>();
+            PinHandler* pinHandler = getModule<PinHandler>();
+
+            auto imu         = sensors->getLSM6DSRXLastSample();
+            auto mag         = sensors->getLIS2MDLLastSample();
+            auto gps         = sensors->getUBXGPSLastSample();
+            auto pressDigi   = sensors->getLPS22DFLastSample();
+            auto pressStatic = sensors->getStaticPressure();
+            auto pressPitot  = sensors->getPitotLastSample();
+
+            tm.timestamp       = TimestampTimer::getTimestamp();
+            tm.pressure_digi   = pressDigi.pressure;
+            tm.pressure_static = pressStatic.pressure;
+            tm.airspeed_pitot  = pressPitot.airspeed;
+            tm.altitude_agl    = -1.0f;  // TODO
+
+            // Sensors
+            tm.acc_x   = imu.accelerationX;
+            tm.acc_y   = imu.accelerationY;
+            tm.acc_z   = imu.accelerationZ;
+            tm.gyro_x  = imu.angularSpeedX;
+            tm.gyro_y  = imu.angularSpeedY;
+            tm.gyro_z  = imu.angularSpeedZ;
+            tm.mag_x   = mag.magneticFieldX;
+            tm.mag_y   = mag.magneticFieldY;
+            tm.mag_z   = mag.magneticFieldZ;
+            tm.gps_lat = gps.latitude;
+            tm.gps_lon = gps.longitude;
+            tm.gps_alt = gps.height;
+            tm.gps_fix = gps.fix;
+
+            // Servos
+            tm.left_servo_angle = getModule<Actuators>()->getServoAngle(
+                ServosList::PARAFOIL_LEFT_SERVO);
+            tm.right_servo_angle = getModule<Actuators>()->getServoAngle(
+                ServosList::PARAFOIL_RIGHT_SERVO);
+
+            // Algorithms
+            tm.nas_n      = -1.0f;  // TODO
+            tm.nas_e      = -1.0f;  // TODO
+            tm.nas_d      = -1.0f;  // TODO
+            tm.nas_vn     = -1.0f;  // TODO
+            tm.nas_ve     = -1.0f;  // TODO
+            tm.nas_vd     = -1.0f;  // TODO
+            tm.nas_qx     = -1.0f;  // TODO
+            tm.nas_qy     = -1.0f;  // TODO
+            tm.nas_qz     = -1.0f;  // TODO
+            tm.nas_qw     = -1.0f;  // TODO
+            tm.nas_bias_x = -1.0f;  // TODO
+            tm.nas_bias_y = -1.0f;  // TODO
+            tm.nas_bias_z = -1.0f;  // TODO
+            tm.wes_n      = -1.0;   // TODO
+            tm.wes_e      = -1.0;   // TODO
+
+            tm.battery_voltage     = sensors->getBatteryVoltage().voltage;
+            tm.cam_battery_voltage = sensors->getCamBatteryVoltage().voltage;
+            tm.temperature         = pressDigi.temperature;
+
+            // State machines
+            tm.fmm_state = static_cast<uint8_t>(
+                getModule<FlightModeManager>()->getStatus().state);
+            tm.nas_state = 255;  // TODO
+            tm.wes_state = 255;  // TODO
+
+            // Pins
+            tm.pin_launch =
+                pinHandler->getPinData(PinList::DETACH_PAYLOAD_PIN).lastState
+                    ? 1
+                    : 0;
+            tm.pin_nosecone =
+                pinHandler->getPinData(PinList::EXPULSION_SENSE).lastState ? 1
+                                                                           : 0;
+            tm.cutter_presence = 255;  // TODO
+
+            mavlink_msg_payload_flight_tm_encode(config::Mavlink::SYSTEM_ID,
+                                                 config::Mavlink::COMPONENT_ID,
+                                                 &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_STATS_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_payload_stats_tm_t tm;
+
+            tm.liftoff_max_acc    = -1.0f;  // TODO
+            tm.liftoff_max_acc_ts = 0;      // TODO
+            tm.dpl_ts             = 0;      // TODO
+            tm.dpl_max_acc        = -1.0f;  // TODO
+            tm.max_z_speed        = -1.0f;  // TODO
+            tm.max_z_speed_ts     = 0;      // TODO
+            tm.max_airspeed_pitot = -1.0f;  // TODO
+            tm.max_speed_altitude = -1.0f;  // TODO
+            tm.apogee_ts          = 0;      // TODO
+            tm.apogee_lat         = -1.0f;  // TODO
+            tm.apogee_lon         = -1.0f;  // TODO
+            tm.apogee_alt         = -1.0f;  // TODO
+            tm.min_pressure       = -1.0f;  // TODO
+
+            // Cpu stuff
+            CpuMeterData cpuStats = CpuMeter::getCpuStats();
+            CpuMeter::resetCpuStats();
+            tm.cpu_load  = cpuStats.mean;
+            tm.free_heap = cpuStats.freeHeap;
+
+            // Log stuff
+            LoggerStats loggerStats = Logger::getInstance().getStats();
+            tm.log_good             = (loggerStats.lastWriteError == 0) ? 1 : 0;
+            tm.log_number           = loggerStats.logNumber;
+
+            tm.main_board_state  = 0;  // TODO
+            tm.motor_board_state = 0;  // TODO
+            tm.main_can_status   = 0;  // TODO
+            tm.motor_can_status  = 0;  // TODO
+            tm.rig_can_status    = 0;  // TODO
+
+            tm.hil_state = 0;  // TODO
+
+            mavlink_msg_payload_stats_tm_encode(config::Mavlink::SYSTEM_ID,
+                                                config::Mavlink::COMPONENT_ID,
+                                                &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        default:
+            return false;
+    }
+}
+
+bool Radio::enqueueSensorsTm(SensorsTMList tmId)
+{
+    switch (tmId)
+    {
+        case MAV_GPS_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_gps_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getUBXGPSLastSample();
+
+            tm.fix          = sample.fix;
+            tm.height       = sample.height;
+            tm.latitude     = sample.latitude;
+            tm.longitude    = sample.longitude;
+            tm.n_satellites = sample.satellites;
+            tm.speed        = sample.speed;
+            tm.timestamp    = sample.gpsTimestamp;
+            tm.track        = sample.track;
+            tm.vel_down     = sample.velocityDown;
+            tm.vel_east     = sample.velocityEast;
+            tm.vel_north    = sample.velocityNorth;
+            strcpy(tm.sensor_name, "UBXGPS");
+
+            mavlink_msg_gps_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_ADS131M08_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_adc_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getADS131M08LastSample();
+
+            tm.channel_0 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
+            tm.channel_1 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
+            tm.channel_2 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
+            tm.channel_3 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
+            tm.channel_4 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
+            tm.channel_5 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
+            tm.channel_6 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
+            tm.channel_7 =
+                sample.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
+            tm.timestamp = sample.timestamp;
+            strcpy(tm.sensor_name, "ADS131M08");
+
+            mavlink_msg_adc_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_BATTERY_VOLTAGE_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_voltage_tm_t tm;
+
+            auto data = getModule<Sensors>()->getBatteryVoltage();
+
+            tm.voltage   = data.voltage;
+            tm.timestamp = data.voltageTimestamp;
+            strcpy(tm.sensor_name, "BATTERY_VOLTAGE");
+
+            mavlink_msg_voltage_tm_encode(config::Mavlink::SYSTEM_ID,
+                                          config::Mavlink::COMPONENT_ID, &msg,
+                                          &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_LPS28DFW_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_pressure_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getLPS28DFWLastSample();
+
+            tm.pressure  = sample.pressure;
+            tm.timestamp = sample.pressureTimestamp;
+            strcpy(tm.sensor_name, "LPS28DFW");
+
+            mavlink_msg_pressure_tm_encode(config::Mavlink::SYSTEM_ID,
+                                           config::Mavlink::COMPONENT_ID, &msg,
+                                           &tm);
+
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_LPS22DF_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_pressure_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getLPS22DFLastSample();
+
+            tm.pressure  = sample.pressure;
+            tm.timestamp = sample.pressureTimestamp;
+            strcpy(tm.sensor_name, "LPS22DF");
+
+            mavlink_msg_pressure_tm_encode(config::Mavlink::SYSTEM_ID,
+                                           config::Mavlink::COMPONENT_ID, &msg,
+                                           &tm);
+
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_LIS2MDL_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_imu_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getLIS2MDLLastSample();
+
+            tm.acc_x     = 0;
+            tm.acc_y     = 0;
+            tm.acc_z     = 0;
+            tm.gyro_x    = 0;
+            tm.gyro_y    = 0;
+            tm.gyro_z    = 0;
+            tm.mag_x     = sample.magneticFieldX;
+            tm.mag_y     = sample.magneticFieldY;
+            tm.mag_z     = sample.magneticFieldZ;
+            tm.timestamp = sample.magneticFieldTimestamp;
+            strcpy(tm.sensor_name, "LIS2MDL");
+
+            mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_LSM6DSRX_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_imu_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getLSM6DSRXLastSample();
+
+            tm.mag_x     = 0;
+            tm.mag_y     = 0;
+            tm.mag_z     = 0;
+            tm.acc_x     = sample.accelerationX;
+            tm.acc_y     = sample.accelerationY;
+            tm.acc_z     = sample.accelerationZ;
+            tm.gyro_x    = sample.angularSpeedX;
+            tm.gyro_y    = sample.angularSpeedY;
+            tm.gyro_z    = sample.angularSpeedZ;
+            tm.timestamp = sample.accelerationTimestamp;
+            strcpy(tm.sensor_name, "LSM6DSRX");
+
+            mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        case MAV_H3LIS331DL_ID:
+        {
+            mavlink_message_t msg;
+            mavlink_imu_tm_t tm;
+
+            auto sample = getModule<Sensors>()->getH3LIS331DLLastSample();
+
+            tm.mag_x     = 0;
+            tm.mag_y     = 0;
+            tm.mag_z     = 0;
+            tm.gyro_x    = 0;
+            tm.gyro_y    = 0;
+            tm.gyro_z    = 0;
+            tm.acc_x     = sample.accelerationX;
+            tm.acc_y     = sample.accelerationY;
+            tm.acc_z     = sample.accelerationZ;
+            tm.timestamp = sample.accelerationTimestamp;
+            strcpy(tm.sensor_name, "H3LIS331DL");
+
+            mavlink_msg_imu_tm_encode(config::Mavlink::SYSTEM_ID,
+                                      config::Mavlink::COMPONENT_ID, &msg, &tm);
+            enqueueMessage(msg);
+            return true;
+        }
+
+        default:
+            return false;
+    }
+}
+
+}  // namespace Payload
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 23a40a77db9f3aa0fce8d7b5c61676ea2645ff16..c51d6d93669bc29ef99de17b0759d18ca8b4b352 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,504 +19,170 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-#include <Payload/Actuators/Actuators.h>
-#include <Payload/AltitudeTrigger/AltitudeTrigger.h>
+
 #include <Payload/BoardScheduler.h>
 #include <Payload/Buses.h>
 #include <Payload/Radio/Radio.h>
-#include <Payload/Radio/RadioData.h>
-#include <Payload/Sensors/Sensors.h>
-#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Payload/StateMachines/NASController/NASController.h>
-#include <Payload/StateMachines/WingController/WingController.h>
-#include <Payload/TMRepository/TMRepository.h>
-#include <common/Events.h>
-#include <common/Topics.h>
-#include <drivers/interrupt/external_interrupts.h>
-#include <events/EventBroker.h>
+#include <common/Radio.h>
 #include <radio/SX1278/SX1278Frontends.h>
 
-#include <memory>
-
 using namespace Boardcore;
 using namespace Common;
+namespace config = Payload::Config::Radio;
 
-#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl
-#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
-#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl
+namespace
+{
 
-static Payload::Radio* staticRadio = nullptr;
+// Static radio instance that will handle the radio interrupts
+SX1278Fsk* staticTransceiver = nullptr;
 
-void __attribute__((used)) SX1278_IRQ_DIO0()
+inline void handleDioIRQ()
 {
-    if (staticRadio && staticRadio->transceiver)
+    if (staticTransceiver)
     {
-        staticRadio->transceiver->handleDioIRQ();
+        staticTransceiver->handleDioIRQ();
     }
 }
 
-void __attribute__((used)) SX1278_IRQ_DIO1()
+}  // namespace
+
+void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); }
+
+namespace Payload
 {
-    if (staticRadio && staticRadio->transceiver)
-    {
-        staticRadio->transceiver->handleDioIRQ();
-    }
-}
 
-void __attribute__((used)) SX1278_IRQ_DIO3()
+Radio::~Radio()
 {
-    if (staticRadio && staticRadio->transceiver)
+    if (staticTransceiver == transceiver.get())
     {
-        staticRadio->transceiver->handleDioIRQ();
+        staticTransceiver = nullptr;
     }
 }
 
-namespace Payload
-{
-
-Radio::Radio() { staticRadio = this; }
-
 bool Radio::start()
 {
+    using namespace Units::Frequency;
+    using namespace std::chrono;
+
     auto& scheduler = getModule<BoardScheduler>()->radio();
 
-    // Config the transceiver
-    SX1278Fsk::Config config = {
-        .freq_rf    = 869000000,
-        .freq_dev   = 50000,
-        .bitrate    = 48000,
-        .rx_bw      = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000,
-        .afc_bw     = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000,
-        .ocp        = 120,
-        .power      = 13,
-        .shaping    = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0,
-        .dc_free    = Boardcore::SX1278Fsk::Config::DcFree::WHITENING,
-        .enable_crc = false};
-
-    std::unique_ptr<SX1278::ISX1278Frontend> frontend =
-        std::make_unique<Skyward433Frontend>();
-
-    transceiver = new SX1278Fsk(
+    // Initialize the radio
+    auto frontend = std::make_unique<Skyward433Frontend>();
+
+    transceiver = std::make_unique<SX1278Fsk>(
         getModule<Buses>()->radio(), miosix::radio::cs::getPin(),
         miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(),
         miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_128,
         std::move(frontend));
 
-    // Config the radio
-    SX1278Fsk::Error error = transceiver->init(config);
+    // Set the static instance for handling radio interrupts
+    staticTransceiver = transceiver.get();
+
+    // Configure the radio
+    if (transceiver->init(PAYLOAD_RADIO_CONFIG) != SX1278Fsk::Error::NONE)
+    {
+        LOG_ERR(logger, "Failed to initialize the radio");
+        return false;
+    }
+
+    // Initialize the Mavlink driver
+    mavDriver = std::make_unique<MavDriver>(
+        transceiver.get(),
+        [this](MavDriver*, const mavlink_message_t& msg)
+        { handleMessage(msg); },
+        milliseconds{config::MavlinkDriver::SLEEP_AFTER_SEND}.count(),
+        milliseconds{config::MavlinkDriver::MAX_PKT_AGE}.count());
 
-    // Add periodic telemetry send task
-    uint8_t result =
-        scheduler.addTask([&]() { this->sendPeriodicMessage(); },
-                          RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD,
-                          TaskScheduler::Policy::RECOVER);
-    result *= scheduler.addTask(
-        [&]()
+    if (!mavDriver->start())
+    {
+        LOG_ERR(logger, "Failed to initialize the Mavlink driver");
+        return false;
+    }
+
+    // Add the high rate telemetry task
+    auto highRateTask = scheduler.addTask(
+        [this]()
         {
-            this->enqueueMsg(
-                getModule<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0));
+            enqueueHighRateTelemetry();
+            flushMessageQueue();
         },
-        RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2,
-        TaskScheduler::Policy::RECOVER);
-
-    // Config mavDriver
-    mavDriver = new MavDriver(
-        transceiver,
-        [=](MavDriver*, const mavlink_message_t& msg)
-        { this->handleMavlinkMessage(msg); },
-        RadioConfig::RADIO_SLEEP_AFTER_SEND,
-        RadioConfig::RADIO_OUT_BUFFER_MAX_AGE);
-
-    // Check radio failure
-    if (error != SX1278Fsk::Error::NONE)
+        Config::Radio::HIGH_RATE_TELEMETRY);
+
+    if (highRateTask == 0)
     {
+        LOG_ERR(logger, "Failed to add the high rate telemetry task");
         return false;
     }
 
-    // Start the mavlink driver thread
-    return mavDriver->start() && result != 0;
+    auto lowRateTask = scheduler.addTask(
+        [this]()
+        {
+            enqueueLowRateTelemetry();
+            flushMessageQueue();
+        },
+        Config::Radio::LOW_RATE_TELEMETRY);
+
+    if (lowRateTask == 0)
+    {
+        LOG_ERR(logger, "Failed to add the low rate telemetry task");
+        return false;
+    }
+
+    started = true;
+    return true;
 }
 
-void Radio::sendAck(const mavlink_message_t& msg)
+bool Radio::isStarted() { return started; }
+
+void Radio::enqueueAck(const mavlink_message_t& msg)
 {
     mavlink_message_t ackMsg;
-    mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
-                            RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid,
+    mavlink_msg_ack_tm_pack(config::Mavlink::SYSTEM_ID,
+                            config::Mavlink::COMPONENT_ID, &ackMsg, msg.msgid,
                             msg.seq);
-    enqueueMsg(ackMsg);
+    enqueueMessage(ackMsg);
 }
 
-void Radio::sendNack(const mavlink_message_t& msg)
+void Radio::enqueueNack(const mavlink_message_t& msg)
 {
     mavlink_message_t nackMsg;
-    mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
-                             RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
-                             msg.seq);
-    enqueueMsg(nackMsg);
+    mavlink_msg_nack_tm_pack(config::Mavlink::SYSTEM_ID,
+                             config::Mavlink::COMPONENT_ID, &nackMsg, msg.msgid,
+                             msg.seq, 0);
+    enqueueMessage(nackMsg);
 }
 
-void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); }
+void Radio::enqueueHighRateTelemetry() { enqueueSystemTm(MAV_FLIGHT_ID); }
 
-bool Radio::isStarted() { return mavDriver->isStarted(); }
+void Radio::enqueueLowRateTelemetry() { enqueueSystemTm(MAV_STATS_ID); }
 
-void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
+void Radio::enqueueMessage(const mavlink_message_t& msg)
 {
-    switch (msg.msgid)
-    {
-        case MAVLINK_MSG_ID_PING_TC:
-        {
-            // Do nothing, just add the ack to the queue
-            break;
-        }
-        case MAVLINK_MSG_ID_COMMAND_TC:
-        {
-            // Let the handle command reply to the message
-            return handleCommand(msg);
-        }
-        case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
-        {
-            SystemTMList tmId = static_cast<SystemTMList>(
-                mavlink_msg_system_tm_request_tc_get_tm_id(&msg));
-
-            if (tmId == SystemTMList::MAV_SENSORS_STATE_ID)
-            {
-                mavlink_message_t msg;
-                mavlink_sensor_state_tm_t tm;
-                auto sensorsState = getModule<Sensors>()->getSensorInfo();
-                for (SensorInfo i : sensorsState)
-                {
-                    strcpy(tm.sensor_name, i.id.c_str());
-                    tm.state = 0;
-                    if (i.isEnabled)
-                    {
-                        tm.state += 1;
-                    }
-                    if (i.isInitialized)
-                    {
-                        tm.state += 2;
-                    }
-                    mavlink_msg_sensor_state_tm_encode(
-                        RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
-                        &msg, &tm);
-                    enqueueMsg(msg);
-                }
-            }
-            else
-            {
-
-                // Add to the queue the respose
-                mavlink_message_t response =
-                    getModule<TMRepository>()->packSystemTm(tmId, msg.msgid,
-                                                            msg.seq);
-
-                // Add the response to the queue
-                enqueueMsg(response);
-
-                // Check if the TM repo answered with a NACK. If so the function
-                // must return to avoid sending a default ack
-                if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-                {
-                    return;
-                }
-            }
-            break;
-        }
-        case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC:
-        {
-            SensorsTMList tmId = static_cast<SensorsTMList>(
-                mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg));
-
-            // Add to the queue the respose
-            mavlink_message_t response =
-                getModule<TMRepository>()->packSensorsTm(tmId, msg.msgid,
-                                                         msg.seq);
-
-            // Add the response to the queue
-            enqueueMsg(response);
-
-            // Check if the TM repo answered with a NACK. If so the function
-            // must return to avoid sending a default ack
-            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-            {
-                return;
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_servo_tm_request_tc_get_servo_id(&msg));
-
-            // Add to the queue the respose
-            mavlink_message_t response = getModule<TMRepository>()->packServoTm(
-                servoId, msg.msgid, msg.seq);
-
-            // Add the response to the queue
-            mavDriver->enqueueMsg(response);
-
-            // Check if the TM repo answered with a NACK. If so the function
-            // must return to avoid sending a default ack
-            if (response.msgid == MAVLINK_MSG_ID_NACK_TM)
-            {
-                return;
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_set_servo_angle_tc_get_servo_id(&msg));
-            float angle = mavlink_msg_set_servo_angle_tc_get_angle(&msg);
-
-            // Move the servo, if it fails send a nack
-            if (!getModule<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode) ||
-                !getModule<Actuators>()->setServoPosition(servoId, angle))
-            {
-                return sendNack(msg);
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_wiggle_servo_tc_get_servo_id(&msg));
-
-            // Send nack if the FMM is not in test mode
-            if (!getModule<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode) ||
-                !getModule<Actuators>()->wiggleServo(servoId))
-            {
-                return sendNack(msg);
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_RESET_SERVO_TC:
-        {
-            ServosList servoId = static_cast<ServosList>(
-                mavlink_msg_reset_servo_tc_get_servo_id(&msg));
-
-            if (!getModule<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode) ||
-                !getModule<Actuators>()->setServoPosition(servoId, 0))
-            {
-                return sendNack(msg);
-            }
-
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC:
-        {
-            float altitude =
-                mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
-
-            getModule<NASController>()->setReferenceAltitude(altitude);
-
-            RadioSetterParameter log{};
-            log.timestamp   = TimestampTimer::getTimestamp();
-            log.refAltitude = altitude;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
-        {
-            float temperature =
-                mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
-
-            getModule<NASController>()->setReferenceTemperature(temperature);
-
-            RadioSetterParameter log{};
-            log.timestamp      = TimestampTimer::getTimestamp();
-            log.refTemperature = temperature;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
-        {
-            float altitude =
-                mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg);
-
-            getModule<AltitudeTrigger>()->setDeploymentAltitude(altitude);
-
-            RadioSetterParameter log{};
-            log.timestamp   = TimestampTimer::getTimestamp();
-            log.depAltitude = altitude;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_ORIENTATION_TC:
-        {
-            float yaw   = mavlink_msg_set_orientation_tc_get_yaw(&msg);
-            float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg);
-            float roll  = mavlink_msg_set_orientation_tc_get_roll(&msg);
-
-            getModule<NASController>()->setOrientation(yaw, pitch, roll);
-
-            RadioSetterParameter log{};
-            log.timestamp = TimestampTimer::getTimestamp();
-            log.yawOri    = yaw;
-            log.pitchOri  = pitch;
-            log.rollOri   = roll;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_COORDINATES_TC:
-        {
-            float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
-            float longitude =
-                mavlink_msg_set_coordinates_tc_get_longitude(&msg);
-
-            getModule<NASController>()->setCoordinates(
-                Eigen::Vector2f(latitude, longitude));
-
-            RadioSetterParameter log{};
-            log.timestamp = TimestampTimer::getTimestamp();
-            log.latCord   = latitude;
-            log.lonCord   = longitude;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC:
-        {
-            float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg);
-            float longitude =
-                mavlink_msg_set_coordinates_tc_get_longitude(&msg);
-
-            getModule<WingController>()->setTargetPosition(
-                Eigen::Vector2f(latitude, longitude));
-
-            RadioSetterParameter log{};
-            log.timestamp     = TimestampTimer::getTimestamp();
-            log.latTargetCord = latitude;
-            log.lonTargetCord = longitude;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_SET_ALGORITHM_TC:
-        {
-            uint8_t algoID =
-                mavlink_msg_set_algorithm_tc_get_algorithm_number(&msg);
-
-            if (!getModule<WingController>()->selectAlgorithm(algoID))
-            {
-                return sendNack(msg);
-            }
-            RadioSetterParameter log{};
-            log.timestamp = TimestampTimer::getTimestamp();
-            log.algoId    = algoID;
-            Logger::getInstance().log(log);
-            break;
-        }
-        case MAVLINK_MSG_ID_RAW_EVENT_TC:
-        {
-            uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
-            uint8_t eventId = mavlink_msg_raw_event_tc_get_event_id(&msg);
-
-            // Send the event only if the flight mode manager is in test mode
-            if (!getModule<FlightModeManager>()->testState(
-                    &FlightModeManager::state_test_mode))
-            {
-                return sendNack(msg);
-            }
-
-            EventBroker::getInstance().post(topicId, eventId);
-            break;
-        }
-    }
-
-    // At the end send the ack message
-    sendAck(msg);
-}
+    Lock<FastMutex> lock(queueMutex);
 
-void Radio::handleCommand(const mavlink_message_t& msg)
-{
-    MavCommandList commandId = static_cast<MavCommandList>(
-        mavlink_msg_command_tc_get_command_id(&msg));
-
-    static const std::map<MavCommandList, Events> commandToEvent{
-        {MAV_CMD_ARM, TMTC_ARM},
-        {MAV_CMD_DISARM, TMTC_DISARM},
-        {MAV_CMD_CALIBRATE, TMTC_CALIBRATE},
-        {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
-        {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
-        {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
-        {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
-        {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
-        {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
-        {MAV_CMD_START_LOGGING, TMTC_START_LOGGING},
-        {MAV_CMD_STOP_LOGGING, TMTC_STOP_LOGGING},
-        {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD},
-        {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE},
-        {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE},
-        {MAV_CMD_START_RECORDING, TMTC_START_RECORDING},
-        {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING},
-    };
-    switch (commandId)
+    // Insert the message inside the queue only if there is enough space
+    if (messageQueueIndex < messageQueue.size())
     {
-        case MAV_CMD_SAVE_CALIBRATION:
-        {
-            // Save the sensor calibration and adopt it
-            if (!getModule<Sensors>()->writeMagCalibration())
-            {
-                return sendNack(msg);
-            }
-            break;
-        }
-        default:
-        {
-            // If the command is not a particular one, look for it inside the
-            // map
-            auto it = commandToEvent.find(commandId);
-
-            if (it != commandToEvent.end())
-            {
-                EventBroker::getInstance().post(it->second, TOPIC_TMTC);
-            }
-            else
-            {
-                return sendNack(msg);
-            }
-        }
+        messageQueue[messageQueueIndex] = msg;
+        messageQueueIndex++;
     }
-    // Acknowledge the message
-    sendAck(msg);
 }
 
-void Radio::sendPeriodicMessage()
+void Radio::flushMessageQueue()
 {
-    // Send all the queue messages
-    {
-        Lock<FastMutex> lock(queueMutex);
-
-        for (uint8_t i = 0; i < messageQueueIndex; i++)
-        {
-            mavDriver->enqueueMsg(messageQueue[i]);
-        }
+    Lock<FastMutex> lock(queueMutex);
 
-        // Reset the index
-        messageQueueIndex = 0;
+    for (uint32_t i = 0; i < messageQueueIndex; i++)
+    {
+        mavDriver->enqueueMsg(messageQueue[i]);
     }
 
-    mavDriver->enqueueMsg(
-        getModule<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0));
+    // Reset the index
+    messageQueueIndex = 0;
 }
 
-void Radio::enqueueMsg(const mavlink_message_t& msg)
-{
-    {
-        Lock<FastMutex> lock(queueMutex);
-
-        // Insert the message inside the queue only if there is enough space
-        if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE)
-        {
-            messageQueue[messageQueueIndex] = msg;
-            messageQueueIndex++;
-        }
-    }
-}
+void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); }
 
 }  // namespace Payload
diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h
index b7be55dd5483994fa1e8fea71ffd1c5bec981c33..b7bff6b637e85b4bf806b64ba0b9206298fc51d3 100644
--- a/src/boards/Payload/Radio/Radio.h
+++ b/src/boards/Payload/Radio/Radio.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro, Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -19,98 +19,84 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #pragma once
 
 #include <Payload/Configs/RadioConfig.h>
-#include <common/MavlinkGemini.h>
+#include <common/Mavlink.h>
 #include <radio/MavlinkDriver/MavlinkDriver.h>
 #include <radio/SX1278/SX1278Fsk.h>
 #include <utils/DependencyManager/DependencyManager.h>
 
 namespace Payload
 {
-using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
-                                           RadioConfig::RADIO_OUT_QUEUE_SIZE,
-                                           RadioConfig::RADIO_MAV_MSG_LENGTH>;
+using MavDriver =
+    Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
+                             Config::Radio::MavlinkDriver::PKT_QUEUE_SIZE,
+                             Config::Radio::MavlinkDriver::MSG_LENGTH>;
 
 class BoardScheduler;
 class Sensors;
 class Buses;
-class TMRepository;
 class FlightModeManager;
 class Actuators;
 class NASController;
 class WingController;
 class AltitudeTrigger;
+class PinHandler;
+class CanHandler;
 
-class Radio
-    : public Boardcore::InjectableWithDeps<
-          BoardScheduler, Sensors, Buses, TMRepository, FlightModeManager,
-          Actuators, NASController, WingController, AltitudeTrigger>
+class Radio : public Boardcore::InjectableWithDeps<
+                  BoardScheduler, Sensors, Buses, FlightModeManager, Actuators,
+                  NASController, WingController, AltitudeTrigger, PinHandler,
+                  CanHandler>
 {
 public:
     /**
-     * @note This class may only be instantiated once. The constructed instance
-     * sets itself as the static instance for handling radio interrupts.
+     * @brief Unsets the static instance for handling radio interrupts, if the
+     * current one was set by this radio instance.
      */
-    Radio();
+    ~Radio();
 
     /**
-     * @brief Starts the MavlinkDriver
+     * @brief Initializes the radio and Mavlink driver, and sets the static
+     * instance for handling radio interrupts.
      */
     [[nodiscard]] bool start();
 
-    /**
-     * @brief Sends via radio an acknowledge message about the parameter passed
-     * message
-     */
-    void sendAck(const mavlink_message_t& msg);
+    bool isStarted();
 
-    /**
-     * @brief Sends via radio an non-acknowledge message about the parameter
-     * passed message
-     */
-    void sendNack(const mavlink_message_t& msg);
+private:
+    void handleMessage(const mavlink_message_t& msg);
+    void handleCommand(const mavlink_message_t& msg);
 
-    /**
-     * @brief Saves the MavlinkDriver and transceiver status
-     */
-    void logStatus();
+    void enqueueAck(const mavlink_message_t& msg);
+    void enqueueNack(const mavlink_message_t& msg);
 
-    /**
-     * @brief Returns if the radio module is correctly started
-     */
-    bool isStarted();
-
-    Boardcore::SX1278Fsk* transceiver = nullptr;
-    MavDriver* mavDriver              = nullptr;
+    bool enqueueSystemTm(SystemTMList tmId);
+    bool enqueueSensorsTm(SensorsTMList sensorId);
 
-private:
-    /**
-     * @brief Called by the MavlinkDriver when a message is received
-     */
-    void handleMavlinkMessage(const mavlink_message_t& msg);
+    void enqueueHighRateTelemetry();
+    void enqueueLowRateTelemetry();
 
-    /**
-     * @brief Called by the handleMavlinkMessage to handle a command message
-     */
-    void handleCommand(const mavlink_message_t& msg);
+    void enqueueMessage(const mavlink_message_t& msg);
+    void flushMessageQueue();
 
     /**
-     * @brief Sends the periodic telemetry
+     * @brief Logs the status of MavlinkDriver and the transceiver
      */
-    void sendPeriodicMessage();
+    void logStatus();
 
-    /**
-     * @brief Inserts the mavlink message into the queue
-     */
-    void enqueueMsg(const mavlink_message_t& msg);
+    std::unique_ptr<Boardcore::SX1278Fsk> transceiver;
+    std::unique_ptr<MavDriver> mavDriver;
 
-    // Messages queue
-    mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE];
+    std::array<mavlink_message_t, Config::Radio::MESSAGE_QUEUE_SIZE>
+        messageQueue;
     uint32_t messageQueueIndex = 0;
     miosix::FastMutex queueMutex;
 
+    std::atomic<bool> started{false};
+
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio");
 };
 
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index 2f8293217cdf79c9916249af836afd08c8064c1a..d7eac21b8c8fdb83e7fbd8a23a8ef36e060e864e 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -132,8 +132,8 @@ void Sensors::calibrate()
     for (auto i = 0; i < config::Calibration::SAMPLE_COUNT; i++)
     {
         lps28dfwStats.add(getLPS28DFWLastSample().pressure);
-        staticPressureStats.add(getStaticPressureLastSample().pressure);
-        dynamicPressureStats.add(getDynamicPressureLastSample().pressure);
+        staticPressureStats.add(getStaticPressure().pressure);
+        dynamicPressureStats.add(getDynamicPressure().pressure);
 
         auto wakeup = start + config::Calibration::SAMPLE_PERIOD;
         miosix::Thread::nanoSleepUntil(wakeup.time_since_epoch().count());
@@ -218,16 +218,14 @@ InternalADCData Sensors::getInternalADCLastSample()
     return internalAdc ? internalAdc->getLastSample() : InternalADCData{};
 }
 
-HSCMRNN015PAData Sensors::getStaticPressureLastSample()
+PressureData Sensors::getStaticPressure()
 {
-    return staticPressure ? staticPressure->getLastSample()
-                          : HSCMRNN015PAData{};
+    return staticPressure ? staticPressure->getLastSample() : PressureData{};
 }
 
-SSCMRNN030PAData Sensors::getDynamicPressureLastSample()
+PressureData Sensors::getDynamicPressure()
 {
-    return dynamicPressure ? dynamicPressure->getLastSample()
-                           : SSCMRNN030PAData{};
+    return dynamicPressure ? dynamicPressure->getLastSample() : PressureData{};
 }
 
 PitotData Sensors::getPitotLastSample()
@@ -240,7 +238,7 @@ IMUData Sensors::getIMULastSample()
     return imu ? imu->getLastSample() : IMUData{};
 }
 
-BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
+BatteryVoltageSensorData Sensors::getBatteryVoltage()
 {
     auto sample = getInternalADCLastSample();
 
@@ -254,7 +252,7 @@ BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
     return data;
 }
 
-BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample()
+BatteryVoltageSensorData Sensors::getCamBatteryVoltage()
 {
     auto sample = getInternalADCLastSample();
 
@@ -307,14 +305,20 @@ std::vector<SensorInfo> Sensors::getSensorInfo()
 {
     if (manager)
     {
-        return {manager->getSensorInfo(lps22df.get()),
-                manager->getSensorInfo(lps28dfw.get()),
-                manager->getSensorInfo(h3lis331dl.get()),
-                manager->getSensorInfo(lis2mdl.get()),
-                manager->getSensorInfo(ubxgps.get()),
-                manager->getSensorInfo(lsm6dsrx.get()),
-                manager->getSensorInfo(ads131m08.get()),
-                manager->getSensorInfo(internalAdc.get())};
+        return {
+            manager->getSensorInfo(lps22df.get()),
+            manager->getSensorInfo(lps28dfw.get()),
+            manager->getSensorInfo(h3lis331dl.get()),
+            manager->getSensorInfo(lis2mdl.get()),
+            manager->getSensorInfo(ubxgps.get()),
+            manager->getSensorInfo(lsm6dsrx.get()),
+            manager->getSensorInfo(ads131m08.get()),
+            manager->getSensorInfo(internalAdc.get()),
+            manager->getSensorInfo(staticPressure.get()),
+            manager->getSensorInfo(dynamicPressure.get()),
+            manager->getSensorInfo(pitot.get()),
+            manager->getSensorInfo(imu.get()),
+        };
     }
     else
     {
@@ -534,8 +538,8 @@ void Sensors::ads131m08Create()
         channel.enabled = false;
     }
     // Enable required channels
-    channels[(uint8_t)config::ADS131M08::STATIC_PRESSURE_CH].enabled  = true;
-    channels[(uint8_t)config::ADS131M08::DYNAMIC_PRESSURE_CH].enabled = true;
+    channels[(uint8_t)config::StaticPressure::ADC_CH].enabled  = true;
+    channels[(uint8_t)config::DynamicPressure::ADC_CH].enabled = true;
 
     ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->ADS131M08(),
                                             hwmap::ADS131M08::cs::getPin(),
@@ -587,12 +591,14 @@ void Sensors::staticPressureCreate()
 
     auto readVoltage = [this]
     {
-        auto sample = ads131m08->getLastSample();
-        return sample.getVoltage(config::ADS131M08::STATIC_PRESSURE_CH);
+        auto sample  = getADS131M08LastSample();
+        auto voltage = sample.getVoltage(config::StaticPressure::ADC_CH);
+        voltage.voltage *= config::StaticPressure::SCALE;
+
+        return voltage;
     };
 
-    // TODO: use updated static pressure driver
-    staticPressure = std::make_unique<HSCMRNN015PA>(readVoltage, 1.2);
+    staticPressure = std::make_unique<MPXH6115A>(readVoltage);
 }
 
 void Sensors::staticPressureInsert(SensorManager::SensorMap_t& map)
@@ -615,12 +621,14 @@ void Sensors::dynamicPressureCreate()
 
     auto readVoltage = [this]
     {
-        auto sample = ads131m08->getLastSample();
-        return sample.getVoltage(config::ADS131M08::DYNAMIC_PRESSURE_CH);
+        auto sample  = getADS131M08LastSample();
+        auto voltage = sample.getVoltage(config::DynamicPressure::ADC_CH);
+        voltage.voltage *= config::DynamicPressure::SCALE;
+
+        return voltage;
     };
 
-    // TODO: use updated dynamic pressure driver
-    dynamicPressure = std::make_unique<SSCMRNN030PA>(readVoltage, 1.2);
+    dynamicPressure = std::make_unique<MPXH6115A>(readVoltage);
 }
 
 void Sensors::dynamicPressureInsert(SensorManager::SensorMap_t& map)
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index ae5d1cda5e2bfe24333f655ed56b4d4598659c19..bfb33e82cfb8b759e6cf1afbf0fd4563005af79c 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -35,8 +35,7 @@
 #include <sensors/UBXGPS/UBXGPSSpi.h>
 #include <sensors/analog/BatteryVoltageSensorData.h>
 #include <sensors/analog/Pitot/Pitot.h>
-#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h>
-#include <sensors/analog/pressure/honeywell/SSCMRNN030PA.h>
+#include <sensors/analog/pressure/nxp/MPXH6115A.h>
 #include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
 #include <utils/DependencyManager/DependencyManager.h>
 
@@ -75,7 +74,6 @@ public:
      */
     bool writeMagCalibration();
 
-    // Hardware Sensors
     Boardcore::LPS22DFData getLPS22DFLastSample();
     Boardcore::LPS28DFWData getLPS28DFWLastSample();
     Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
@@ -85,15 +83,14 @@ public:
     Boardcore::ADS131M08Data getADS131M08LastSample();
     Boardcore::InternalADCData getInternalADCLastSample();
 
-    // Software Sensors
-    Boardcore::HSCMRNN015PAData getStaticPressureLastSample();
-    Boardcore::SSCMRNN030PAData getDynamicPressureLastSample();
+    Boardcore::PressureData getStaticPressure();
+    Boardcore::PressureData getDynamicPressure();
+
     Boardcore::PitotData getPitotLastSample();
     IMUData getIMULastSample();
 
-    // Processed values
-    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
-    Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
+    Boardcore::BatteryVoltageSensorData getBatteryVoltage();
+    Boardcore::BatteryVoltageSensorData getCamBatteryVoltage();
     Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
 
     void pitotSetReferenceAltitude(float altitude);
@@ -163,7 +160,6 @@ protected:
      */
     virtual bool postSensorCreationHook() { return true; }
 
-    // Hardware sensor instances
     std::unique_ptr<Boardcore::LPS22DF> lps22df;
     std::unique_ptr<Boardcore::LPS28DFW> lps28dfw;
     std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
@@ -173,9 +169,8 @@ protected:
     std::unique_ptr<Boardcore::ADS131M08> ads131m08;
     std::unique_ptr<Boardcore::InternalADC> internalAdc;
 
-    // Software sensor instances
-    std::unique_ptr<Boardcore::HSCMRNN015PA> staticPressure;
-    std::unique_ptr<Boardcore::SSCMRNN030PA> dynamicPressure;
+    std::unique_ptr<Boardcore::MPXH6115A> staticPressure;
+    std::unique_ptr<Boardcore::MPXH6115A> dynamicPressure;
     std::unique_ptr<Boardcore::Pitot> pitot;
     std::unique_ptr<Payload::RotatedIMU> imu;
 
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index bbfa7c3684dece17890ac26be74807f672160840..c82842422059c9355ff679ff762d7049e1b0a2ef 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -145,8 +145,7 @@ void NASController::calibrate()
                      magData.magneticFieldZ);
 
         // Static pressure barometer
-        HSCMRNN015PAData barometerData =
-            getModule<Sensors>()->getStaticPressureLastSample();
+        PressureData barometerData = getModule<Sensors>()->getStaticPressure();
         pressure.add(barometerData.pressure);
 
         miosix::Thread::sleep(NASConfig::CALIBRATION_SLEEP_TIME);
diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp
deleted file mode 100644
index d891da603fa7ddbbb961e373a23de2fb10822c8d..0000000000000000000000000000000000000000
--- a/src/boards/Payload/TMRepository/TMRepository.cpp
+++ /dev/null
@@ -1,594 +0,0 @@
-/* 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 <Payload/Actuators/Actuators.h>
-#include <Payload/BoardScheduler.h>
-#include <Payload/Configs/RadioConfig.h>
-#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
-#include <Payload/PinHandler/PinHandler.h>
-#include <Payload/Radio/Radio.h>
-#include <Payload/Sensors/Sensors.h>
-#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Payload/StateMachines/NASController/NASController.h>
-#include <Payload/StateMachines/WingController/WingController.h>
-#include <Payload/TMRepository/TMRepository.h>
-#include <Payload/WindEstimationScheme/WindEstimation.h>
-#include <diagnostic/CpuMeter/CpuMeter.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <events/EventBroker.h>
-#include <interfaces-impl/hwmapping.h>
-
-using namespace miosix;
-using namespace Boardcore;
-
-namespace Payload
-{
-mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
-                                             uint8_t seq)
-{
-    mavlink_message_t msg;
-
-    switch (tmId)
-    {
-        case SystemTMList::MAV_SYS_ID:
-        {
-            mavlink_sys_tm_t tm;
-
-            tm.timestamp       = TimestampTimer::getTimestamp();
-            tm.logger          = Logger::getInstance().isStarted();
-            tm.board_scheduler = getModule<BoardScheduler>()->isStarted();
-            tm.event_broker    = EventBroker::getInstance().isRunning();
-            tm.radio           = getModule<Radio>()->isStarted();
-            tm.sensors         = getModule<Sensors>()->isStarted();
-            tm.pin_observer    = getModule<PinHandler>()->isStarted();
-
-            mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SystemTMList::MAV_LOGGER_ID:
-        {
-            mavlink_logger_tm_t tm;
-
-            // Get the logger stats
-            LoggerStats stats = Logger::getInstance().getStats();
-
-            tm.timestamp          = stats.timestamp;
-            tm.average_write_time = stats.averageWriteTime;
-            tm.buffers_filled     = stats.buffersFilled;
-            tm.buffers_written    = stats.buffersWritten;
-            tm.dropped_samples    = stats.droppedSamples;
-            tm.last_write_error   = stats.lastWriteError;
-            tm.log_number         = stats.logNumber;
-            tm.max_write_time     = stats.maxWriteTime;
-            tm.queued_samples     = stats.queuedSamples;
-            tm.too_large_samples  = stats.tooLargeSamples;
-            tm.writes_failed      = stats.writesFailed;
-
-            mavlink_msg_logger_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                         RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SystemTMList::MAV_MAVLINK_STATS:
-        {
-            mavlink_mavlink_stats_tm_t tm;
-
-            // Get the mavlink stats
-            MavlinkStatus stats = getModule<Radio>()->mavDriver->getStatus();
-
-            tm.timestamp               = stats.timestamp;
-            tm.n_send_queue            = stats.nSendQueue;
-            tm.max_send_queue          = stats.maxSendQueue;
-            tm.n_send_errors           = stats.nSendErrors;
-            tm.msg_received            = stats.mavStats.msg_received;
-            tm.buffer_overrun          = stats.mavStats.buffer_overrun;
-            tm.parse_error             = stats.mavStats.parse_error;
-            tm.parse_state             = stats.mavStats.parse_state;
-            tm.packet_idx              = stats.mavStats.packet_idx;
-            tm.current_rx_seq          = stats.mavStats.current_rx_seq;
-            tm.current_tx_seq          = stats.mavStats.current_tx_seq;
-            tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count;
-            tm.packet_rx_drop_count    = stats.mavStats.packet_rx_drop_count;
-
-            mavlink_msg_mavlink_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                                RadioConfig::MAV_COMP_ID, &msg,
-                                                &tm);
-            break;
-        }
-        case SystemTMList::MAV_NAS_ID:
-        {
-            mavlink_nas_tm_t tm;
-
-            // Get the current NAS state
-            NASState state = getModule<NASController>()->getNasState();
-            ReferenceValues ref =
-                getModule<NASController>()->getReferenceValues();
-
-            tm.timestamp = state.timestamp;
-            tm.state     = static_cast<uint8_t>(
-                getModule<NASController>()->getStatus().state);
-            tm.nas_n           = state.n;
-            tm.nas_e           = state.e;
-            tm.nas_d           = state.d;
-            tm.nas_vn          = state.vn;
-            tm.nas_ve          = state.ve;
-            tm.nas_vd          = state.vd;
-            tm.nas_qx          = state.qx;
-            tm.nas_qy          = state.qy;
-            tm.nas_qz          = state.qz;
-            tm.nas_qw          = state.qw;
-            tm.nas_bias_x      = state.bx;
-            tm.nas_bias_y      = state.by;
-            tm.nas_bias_z      = state.bz;
-            tm.ref_pressure    = ref.refPressure;
-            tm.ref_temperature = ref.refTemperature;
-            tm.ref_latitude    = ref.refLatitude;
-            tm.ref_longitude   = ref.refLongitude;
-
-            mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SystemTMList::MAV_FLIGHT_ID:
-        {
-            mavlink_payload_flight_tm_t tm;
-
-            tm.timestamp = TimestampTimer::getTimestamp();
-
-            // Last samples
-            LPS28DFWData lps28dfw =
-                getModule<Sensors>()->getLPS28DFWLastSample();
-            IMUData imu    = getModule<Sensors>()->getIMULastSample();
-            UBXGPSData gps = getModule<Sensors>()->getUBXGPSLastSample();
-            Eigen::Vector2f wind =
-                getModule<WindEstimation>()->getWindEstimationScheme();
-
-            // NAS state
-            NASState nasState = getModule<NASController>()->getNasState();
-
-            tm.nas_state = static_cast<uint8_t>(
-                getModule<NASController>()->getStatus().state);
-            tm.fmm_state = static_cast<uint8_t>(
-                getModule<FlightModeManager>()->getStatus().state);
-            tm.wes_state = static_cast<uint8_t>(
-                getModule<WingController>()->getStatus().state);
-
-            tm.wes_n = wind[0];
-            tm.wes_e = wind[1];
-
-            tm.pressure_digi = lps28dfw.pressure;
-            tm.pressure_static =
-                getModule<Sensors>()->getStaticPressureLastSample().pressure;
-            // Pitot
-            tm.airspeed_pitot =
-                getModule<Sensors>()->getPitotLastSample().airspeed;
-
-            // Altitude agl
-            tm.altitude_agl = -nasState.d;
-
-            // IMU
-            tm.acc_x  = imu.accData.accelerationX;
-            tm.acc_y  = imu.accData.accelerationY;
-            tm.acc_z  = imu.accData.accelerationZ;
-            tm.gyro_x = imu.gyroData.angularSpeedX;
-            tm.gyro_y = imu.gyroData.angularSpeedY;
-            tm.gyro_z = imu.gyroData.angularSpeedZ;
-
-            // Magnetometer
-            tm.mag_x = imu.magData.magneticFieldX;
-            tm.mag_y = imu.magData.magneticFieldY;
-            tm.mag_z = imu.magData.magneticFieldZ;
-
-            // GPS
-            tm.gps_fix = gps.fix;
-            tm.gps_lat = gps.latitude;
-            tm.gps_lon = gps.longitude;
-            tm.gps_alt = gps.height;
-
-            // NAS
-            tm.nas_n      = nasState.n;
-            tm.nas_e      = nasState.e;
-            tm.nas_d      = nasState.d;
-            tm.nas_vn     = nasState.vn;
-            tm.nas_ve     = nasState.ve;
-            tm.nas_vd     = nasState.vd;
-            tm.nas_qx     = nasState.qx;
-            tm.nas_qy     = nasState.qy;
-            tm.nas_qz     = nasState.qz;
-            tm.nas_qw     = nasState.qw;
-            tm.nas_bias_x = nasState.bx;
-            tm.nas_bias_y = nasState.by;
-            tm.nas_bias_z = nasState.bz;
-            // Servos
-            tm.left_servo_angle = getModule<Actuators>()->getServoAngle(
-                ServosList::PARAFOIL_LEFT_SERVO);
-            tm.right_servo_angle = getModule<Actuators>()->getServoAngle(
-                ServosList::PARAFOIL_RIGHT_SERVO);
-
-            tm.pin_nosecone = getModule<PinHandler>()
-                                  ->getPinData()[PinsList::NOSECONE_PIN]
-                                  .lastState;
-
-            tm.cutter_presence =
-                static_cast<uint8_t>(adcs::cutterSense::value());
-
-            tm.battery_voltage =
-                getModule<Sensors>()->getBatteryVoltageLastSample().batVoltage;
-            tm.current_consumption = 0;
-            tm.temperature         = lps28dfw.temperature;
-            tm.logger_error = Logger::getInstance().getStats().lastWriteError;
-
-            tm.cam_battery_voltage = getModule<Sensors>()
-                                         ->getCamBatteryVoltageLastSample()
-                                         .batVoltage;
-
-            mavlink_msg_payload_flight_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                                 RadioConfig::MAV_COMP_ID, &msg,
-                                                 &tm);
-
-            break;
-        }
-        case SystemTMList::MAV_STATS_ID:
-        {
-            mavlink_payload_stats_tm_t tm =
-                getModule<FlightStatsRecorder>()->getStats();
-            tm.cpu_load  = CpuMeter::getCpuStats().mean;
-            tm.free_heap = CpuMeter::getCpuStats().freeHeap;
-
-            mavlink_msg_payload_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                                RadioConfig::MAV_COMP_ID, &msg,
-                                                &tm);
-            break;
-        }
-        case SystemTMList::MAV_FSM_ID:
-        {
-            mavlink_fsm_tm_t tm;
-
-            tm.timestamp = TimestampTimer::getTimestamp();
-            tm.abk_state = 0;
-            tm.ada_state = 0;
-            tm.dpl_state = static_cast<uint8_t>(
-                getModule<WingController>()->getStatus().state);
-            tm.fmm_state = static_cast<uint8_t>(
-                getModule<FlightModeManager>()->getStatus().state);
-            tm.nas_state = static_cast<uint8_t>(
-                getModule<NASController>()->getStatus().state);
-            tm.wes_state = 0;
-
-            mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SystemTMList::MAV_PIN_OBS_ID:
-        {
-            mavlink_pin_tm_t tm;
-
-            tm.timestamp = TimestampTimer::getTimestamp();
-            tm.last_change_timestamp =
-                getModule<PinHandler>()
-                    ->getPinData()[PinsList::NOSECONE_PIN]
-                    .lastStateTimestamp; /*<  Last change timestamp of pin*/
-            tm.pin_id =
-                PinsList::NOSECONE_PIN; /*<  A member of the PinsList enum*/
-            tm.changes_counter =
-                getModule<PinHandler>()
-                    ->getPinData()[PinsList::NOSECONE_PIN]
-                    .changesCount; /*<  Number of changes of pin*/
-            tm.current_state = getModule<PinHandler>()
-                                   ->getPinData()[PinsList::NOSECONE_PIN]
-                                   .lastState; /*<  Current state of pin*/
-
-            mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        default:
-        {
-            // Send by default the nack message
-            mavlink_nack_tm_t nack;
-
-            nack.recv_msgid = msgId;
-            nack.seq_ack    = seq;
-
-            LOG_ERR(logger, "Unknown message id: {}", tmId);
-            mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                       RadioConfig::MAV_COMP_ID, &msg, &nack);
-
-            break;
-        }
-    }
-    return msg;
-}
-mavlink_message_t TMRepository::packSensorsTm(SensorsTMList sensorId,
-                                              uint8_t msgId, uint8_t seq)
-{
-    mavlink_message_t msg;
-
-    switch (sensorId)
-    {
-        case SensorsTMList::MAV_GPS_ID:
-        {
-            mavlink_gps_tm_t tm;
-
-            UBXGPSData gpsData = getModule<Sensors>()->getUBXGPSLastSample();
-
-            tm.timestamp = gpsData.gpsTimestamp;
-            strcpy(tm.sensor_name, "UBXGPS");
-            tm.fix          = gpsData.fix;
-            tm.height       = gpsData.height;
-            tm.latitude     = gpsData.latitude;
-            tm.longitude    = gpsData.longitude;
-            tm.n_satellites = gpsData.satellites;
-            tm.speed        = gpsData.speed;
-            tm.track        = gpsData.track;
-            tm.vel_down     = gpsData.velocityDown;
-            tm.vel_east     = gpsData.velocityEast;
-            tm.vel_north    = gpsData.velocityNorth;
-
-            mavlink_msg_gps_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-
-        case SensorsTMList::MAV_STATIC_PRESS_ID:
-        {
-            mavlink_pressure_tm_t tm;
-
-            HSCMRNN015PAData pressureData =
-                getModule<Sensors>()->getStaticPressureLastSample();
-
-            tm.timestamp = pressureData.pressureTimestamp;
-            strcpy(tm.sensor_name, "STATIC_PRESSURE");
-            tm.pressure = pressureData.pressure;
-
-            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SensorsTMList::MAV_DPL_PRESS_ID:
-        {
-            mavlink_pressure_tm_t tm;
-
-            SSCMRNN030PAData pressureData =
-                getModule<Sensors>()->getDynamicPressureLastSample();
-
-            tm.timestamp = pressureData.pressureTimestamp;
-            strcpy(tm.sensor_name, "DYNAMIC_PRESSURE");
-            tm.pressure = pressureData.pressure;
-
-            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
-
-            break;
-        }
-        case SensorsTMList::MAV_PITOT_PRESS_ID:
-        {
-            mavlink_pressure_tm_t tm;
-
-            SSCMRNN030PAData pitot =
-                getModule<Sensors>()->getDynamicPressureLastSample();
-
-            tm.timestamp = pitot.pressureTimestamp;
-            tm.pressure  = pitot.pressure;
-            strcpy(tm.sensor_name, "PITOT_PRESSURE");
-
-            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_LIS2MDL_ID:
-        {
-            mavlink_imu_tm_t tm;
-
-            LIS2MDLData mag = getModule<Sensors>()->getLIS2MDLLastSample();
-
-            tm.acc_x     = 0;
-            tm.acc_y     = 0;
-            tm.acc_z     = 0;
-            tm.gyro_x    = 0;
-            tm.gyro_y    = 0;
-            tm.gyro_z    = 0;
-            tm.mag_x     = mag.magneticFieldX;
-            tm.mag_y     = mag.magneticFieldY;
-            tm.mag_z     = mag.magneticFieldZ;
-            tm.timestamp = mag.magneticFieldTimestamp;
-            strcpy(tm.sensor_name, "LIS2MD");
-
-            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_LSM6DSRX_ID:
-        {
-            mavlink_imu_tm_t tm;
-
-            LSM6DSRXData imu = getModule<Sensors>()->getLSM6DSRXLastSample();
-
-            tm.acc_x  = imu.accelerationX;
-            tm.acc_y  = imu.accelerationY;
-            tm.acc_z  = imu.accelerationZ;
-            tm.gyro_x = imu.angularSpeedX;
-            tm.gyro_y = imu.angularSpeedY;
-            tm.gyro_z = imu.angularSpeedZ;
-            tm.mag_x  = 0;
-            tm.mag_y  = 0;
-            tm.mag_z  = 0;
-
-            tm.timestamp = imu.accelerationTimestamp;
-            strcpy(tm.sensor_name, "LSM6DSRX");
-
-            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_H3LIS331DL_ID:
-        {
-            mavlink_imu_tm_t tm;
-
-            H3LIS331DLData imu =
-                getModule<Sensors>()->getH3LIS331DLLastSample();
-
-            tm.acc_x  = imu.accelerationX;
-            tm.acc_y  = imu.accelerationY;
-            tm.acc_z  = imu.accelerationZ;
-            tm.gyro_x = 0;
-            tm.gyro_y = 0;
-            tm.gyro_z = 0;
-            tm.mag_x  = 0;
-            tm.mag_y  = 0;
-            tm.mag_z  = 0;
-
-            tm.timestamp = imu.accelerationTimestamp;
-            strcpy(tm.sensor_name, "H3LIS331DL");
-
-            mavlink_msg_imu_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_LPS22DF_ID:
-        {
-            mavlink_pressure_tm_t tm;
-
-            LPS22DFData pressure = getModule<Sensors>()->getLPS22DFLastSample();
-
-            tm.timestamp = pressure.pressureTimestamp;
-            tm.pressure  = pressure.pressure;
-            strcpy(tm.sensor_name, "LPS22DF");
-
-            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_LPS28DFW_ID:
-        {
-            mavlink_pressure_tm_t tm;
-            LPS28DFWData pressure =
-                getModule<Sensors>()->getLPS28DFWLastSample();
-
-            tm.timestamp = pressure.pressureTimestamp;
-            tm.pressure  = pressure.pressure;
-            strcpy(tm.sensor_name, "LPS28DFW");
-
-            mavlink_msg_pressure_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                           RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_BATTERY_VOLTAGE_ID:
-        {
-            mavlink_voltage_tm_t tm;
-            BatteryVoltageSensorData voltage =
-                getModule<Sensors>()->getBatteryVoltageLastSample();
-            tm.voltage = voltage.batVoltage;
-            strcpy(tm.sensor_name, "BATTERY");
-            tm.timestamp = voltage.voltageTimestamp;
-            mavlink_msg_voltage_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                          RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        case SensorsTMList::MAV_ADS_ID:
-        {
-            mavlink_adc_tm_t tm;
-            ADS131M08Data voltage =
-                getModule<Sensors>()->getADS131M08LastSample();
-            tm.channel_0 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_0).voltage;
-
-            tm.channel_1 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage;
-
-            tm.channel_2 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_2).voltage;
-
-            tm.channel_3 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_3).voltage;
-
-            tm.channel_4 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_4).voltage;
-
-            tm.channel_5 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage;
-
-            tm.channel_6 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage;
-
-            tm.channel_7 =
-                voltage.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage;
-
-            strcpy(tm.sensor_name, "ADS131M08");
-            tm.timestamp = voltage.timestamp;
-            mavlink_msg_adc_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                      RadioConfig::MAV_COMP_ID, &msg, &tm);
-            break;
-        }
-        default:
-        {
-            mavlink_nack_tm_t nack;
-
-            nack.recv_msgid = msgId;
-            nack.seq_ack    = seq;
-
-            LOG_DEBUG(logger, "Unknown telemetry id: {}", sensorId);
-            mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                       RadioConfig::MAV_COMP_ID, &msg, &nack);
-            break;
-        }
-    }
-
-    return msg;
-}
-
-mavlink_message_t TMRepository::packServoTm(ServosList servoId, uint8_t msgId,
-                                            uint8_t seq)
-{
-    mavlink_message_t msg;
-
-    if (servoId == PARAFOIL_LEFT_SERVO || servoId == PARAFOIL_RIGHT_SERVO)
-    {
-        mavlink_servo_tm_t tm;
-
-        tm.servo_id       = servoId;
-        tm.servo_position = getModule<Actuators>()->getServoAngle(servoId);
-
-        mavlink_msg_servo_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                    RadioConfig::MAV_COMP_ID, &msg, &tm);
-    }
-    else
-    {
-        mavlink_nack_tm_t nack;
-
-        nack.recv_msgid = msgId;
-        nack.seq_ack    = seq;
-
-        mavlink_msg_nack_tm_encode(RadioConfig::MAV_SYSTEM_ID,
-                                   RadioConfig::MAV_COMP_ID, &msg, &nack);
-    }
-
-    return msg;
-}
-
-}  // namespace Payload
diff --git a/src/boards/Payload/TMRepository/TMRepository.h b/src/boards/Payload/TMRepository/TMRepository.h
deleted file mode 100644
index fbffe14a2882741d80e95fa4cf6e3db2aa1e58cc..0000000000000000000000000000000000000000
--- a/src/boards/Payload/TMRepository/TMRepository.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#pragma once
-
-#include <common/MavlinkGemini.h>
-#include <diagnostic/PrintLogger.h>
-#include <utils/DependencyManager/DependencyManager.h>
-
-namespace Payload
-{
-class Sensors;
-class Radio;
-class BoardScheduler;
-class PinHandler;
-class NASController;
-class FlightModeManager;
-class WingController;
-class WindEstimation;
-class Actuators;
-class FlightStatsRecorder;
-
-class TMRepository : public Boardcore::InjectableWithDeps<
-                         Sensors, Radio, BoardScheduler, PinHandler,
-                         NASController, FlightModeManager, WingController,
-                         WindEstimation, Actuators, FlightStatsRecorder>
-{
-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 Payload
\ No newline at end of file
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index 4636eb2ef93d020e8c472be8a4252fc61919d8be..ac50118173f9743ce900282622e758e3d1e847f4 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -33,7 +33,6 @@
 #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <Payload/StateMachines/NASController/NASController.h>
 #include <Payload/StateMachines/WingController/WingController.h>
-#include <Payload/TMRepository/TMRepository.h>
 #include <Payload/VerticalVelocityTrigger/VerticalVelocityTrigger.h>
 #include <Payload/WindEstimationScheme/WindEstimation.h>
 #include <common/Events.h>
@@ -119,18 +118,17 @@ int main()
 
     // Statistics
     auto statsRecorder = new FlightStatsRecorder();
-    auto tmRepository  = new TMRepository();
 
     // Insert modules
-    bool initResult =
-        depman.insert(buses) && depman.insert(scheduler) &&
-        depman.insert(flightModeManager) && depman.insert(nas) &&
-        depman.insert(sensors) && depman.insert(pinHandler) &&
-        depman.insert(radio) && depman.insert(canHandler) &&
-        depman.insert(altitudeTrigger) && depman.insert(wingController) &&
-        depman.insert(verticalVelocityTrigger) &&
-        depman.insert(windEstimation) && depman.insert(actuators) &&
-        depman.insert(statsRecorder) && depman.insert(tmRepository);
+    bool initResult = depman.insert(buses) && depman.insert(scheduler) &&
+                      depman.insert(flightModeManager) && depman.insert(nas) &&
+                      depman.insert(sensors) && depman.insert(pinHandler) &&
+                      depman.insert(radio) && depman.insert(canHandler) &&
+                      depman.insert(altitudeTrigger) &&
+                      depman.insert(wingController) &&
+                      depman.insert(verticalVelocityTrigger) &&
+                      depman.insert(windEstimation) &&
+                      depman.insert(actuators) && depman.insert(statsRecorder);
 
     // Populate module dependencies
     initResult &= depman.inject();
@@ -147,7 +145,7 @@ int main()
     // Start module instances
     START_MODULE(sensors) { miosix::led1On(); }
     START_MODULE(pinHandler);
-    // START_MODULE(radio) { miosix::led2On(); }
+    START_MODULE(radio) { miosix::led2On(); }
     // START_MODULE(canHandler) { miosix::led3On(); }
     START_MODULE(flightModeManager);
     START_MODULE(nas);