diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 3d649ecc4da0c698bc384c7b48593a11fa4cd18c..fe43c27a9d05fdec48b0bdfad7e6ec8dfa2ec2b0 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -23,8 +23,12 @@
 #include "Radio.h"
 
 #include <RIGv2/Buses.h>
+#include <RIGv2/Sensors/Sensors.h>
 #include <common/Radio.h>
+#include <events/EventBroker.h>
 #include <radio/SX1278/SX1278Frontends.h>
+// TODO(davide.mor): Remove TimestampTimer
+#include <drivers/timer/TimestampTimer.h>
 
 #include <atomic>
 
@@ -33,21 +37,29 @@ using namespace miosix;
 using namespace Common;
 using namespace RIGv2;
 
-Boardcore::SX1278Lora* gRadio{nullptr};
+SX1278Lora* gRadio{nullptr};
 
 void handleDioIRQ()
 {
-    Boardcore::SX1278Lora* instance = gRadio;
+    SX1278Lora* instance = gRadio;
     if (instance)
     {
         instance->handleDioIRQ();
     }
 }
 
+void setIRQRadio(SX1278Lora* radio)
+{
+    FastInterruptDisableLock dl;
+    gRadio = radio;
+}
+
 void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); }
 void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); }
 void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); }
 
+bool Radio::isStarted() { return started; }
+
 bool Radio::start()
 {
     ModuleManager& modules = ModuleManager::getInstance();
@@ -64,10 +76,7 @@ bool Radio::start()
         SPI::ClockDivider::DIV_64, std::move(frontend));
 
     // Store the global radio instance
-    {
-        miosix::FastDisableInterrupt di;
-        gRadio = radio.get();
-    }
+    setIRQRadio(radio.get());
 
     // Initialize radio
     auto result = radio->init(RIG_RADIO_CONFIG);
@@ -91,20 +100,20 @@ bool Radio::start()
         return false;
     }
 
+    started = true;
     return true;
 }
 
 void Radio::stop()
 {
     // Remove global radio instance
-    {
-        miosix::FastDisableInterrupt di;
-        gRadio = nullptr;
-    }
+    setIRQRadio(nullptr);
+
     mavDriver->stop();
+    started = false;
 }
 
-void Radio::queuePacket(const mavlink_message_t& msg)
+void Radio::enqueuePacket(const mavlink_message_t& msg)
 {
     queuedPackets.put(msg);
 }
@@ -132,7 +141,7 @@ void Radio::sendAck(const mavlink_message_t& msg)
     mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
                             Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid,
                             msg.seq);
-    queuePacket(ackMsg);
+    enqueuePacket(ackMsg);
 }
 
 void Radio::sendNack(const mavlink_message_t& msg)
@@ -141,7 +150,7 @@ void Radio::sendNack(const mavlink_message_t& msg)
     mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID,
                              Config::Radio::MAV_COMPONENT_ID, &nackMsg,
                              msg.msgid, msg.seq);
-    queuePacket(nackMsg);
+    enqueuePacket(nackMsg);
 }
 
 Boardcore::MavlinkStatus Radio::getMavStatus()
@@ -154,46 +163,75 @@ void Radio::handleMessage(const mavlink_message_t& msg)
     switch (msg.msgid)
     {
         case MAVLINK_MSG_ID_PING_TC:
+        {
             sendAck(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_COMMAND_TC:
+        {
             handleCommand(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_CONRIG_STATE_TC:
+        {
             handleConrigState(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC:
-            // TODO: Implement SystemTM
-            sendNack(msg);
+        {
+            uint8_t tmId = mavlink_msg_system_tm_request_tc_get_tm_id(&msg);
+
+            mavlink_message_t tm;
+            if (packSystemTm(tmId, tm))
+            {
+                sendAck(msg);
+                enqueuePacket(tm);
+            }
+            else
+            {
+                sendNack(msg);
+            }
+
             break;
+        }
 
         case MAVLINK_MSG_ID_WIGGLE_SERVO_TC:
+        {
             // TODO: Implement servo wiggle
             sendNack(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_SET_ATOMIC_VALVE_TIMING_TC:
+        {
             // TODO: Implement set atomic valve timing
             sendNack(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_SET_VALVE_MAXIMUM_APERTURE_TC:
+        {
             // TODO: Implement set valve maximum aperture
             sendNack(msg);
             break;
+        }
 
         case MAVLINK_MSG_ID_SET_IGNITION_TIME_TC:
+        {
             // TODO: Implement set ignition time
             sendNack(msg);
             break;
+        }
 
         default:
+        {
             // Unrecognized packet
             sendNack(msg);
             break;
+        }
     }
 }
 
@@ -203,6 +241,7 @@ void Radio::handleCommand(const mavlink_message_t& msg)
     switch (cmd)
     {
         case MAV_CMD_START_LOGGING:
+        {
             if (!Logger::getInstance().start())
             {
                 sendNack(msg);
@@ -212,45 +251,154 @@ void Radio::handleCommand(const mavlink_message_t& msg)
                 sendAck(msg);
             }
             break;
+        }
 
         case MAV_CMD_STOP_LOGGING:
+        {
             Logger::getInstance().stop();
             sendAck(msg);
             break;
+        }
 
         case MAV_CMD_CALIBRATE:
+        {
             // TODO: Implement calibrate
             sendNack(msg);
             break;
+        }
 
         default:
+        {
             // Unrecognized command
             sendNack(msg);
             break;
+        }
     }
 }
 
-void Radio::handleConrigState(const mavlink_message_t& msg)
+bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
 {
-    sendAck(msg);
+    ModuleManager& modules = ModuleManager::getInstance();
+    switch (tmId)
+    {
+        case MAV_SYS_ID:
+        {
+            mavlink_sys_tm_t tm;
+
+            tm.timestamp    = TimestampTimer::getTimestamp();
+            tm.logger       = Logger::getInstance().isStarted();
+            tm.event_broker = EventBroker::getInstance().isRunning();
+            // What? Why is this here? Of course the radio is started!
+            tm.radio           = isStarted();
+            tm.sensors         = modules.get<Sensors>()->isStarted();
+            tm.board_scheduler = 0;  // TODO(davide.mor): No BoardScheduler yet
+
+            mavlink_msg_sys_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                      Config::Radio::MAV_COMPONENT_ID, &msg,
+                                      &tm);
+            return true;
+        }
 
-    // TODO: Send actual data
-    sendFakeGseTm();
+        case MAV_LOGGER_ID:
+        {
+            mavlink_logger_tm_t tm;
+
+            LoggerStats stats = Logger::getInstance().getStats();
+
+            tm.timestamp          = TimestampTimer::getTimestamp();
+            tm.log_number         = stats.logNumber;
+            tm.too_large_samples  = stats.tooLargeSamples;
+            tm.dropped_samples    = stats.droppedSamples;
+            tm.queued_samples     = stats.queuedSamples;
+            tm.buffers_filled     = stats.buffersFilled;
+            tm.buffers_written    = stats.buffersWritten;
+            tm.writes_failed      = stats.writesFailed;
+            tm.last_write_error   = stats.lastWriteError;
+            tm.average_write_time = stats.averageWriteTime;
+            tm.max_write_time     = stats.maxWriteTime;
+
+            mavlink_msg_logger_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                         Config::Radio::MAV_COMPONENT_ID, &msg,
+                                         &tm);
+            return true;
+        }
 
-    flushPackets();
+        case MAV_MAVLINK_STATS:
+        {
+            mavlink_mavlink_stats_tm_t tm;
+
+            MavlinkStatus stats = modules.get<Radio>()->mavDriver->getStatus();
+
+            tm.timestamp               = stats.timestamp;
+            tm.n_send_queue            = stats.nSendQueue;
+            tm.max_send_queue          = stats.maxSendQueue;
+            tm.n_send_errors           = stats.nSendErrors;
+            tm.msg_received            = stats.mavStats.msg_received;
+            tm.buffer_overrun          = stats.mavStats.buffer_overrun;
+            tm.parse_error             = stats.mavStats.parse_error;
+            tm.parse_state             = stats.mavStats.parse_state;
+            tm.packet_idx              = stats.mavStats.packet_idx;
+            tm.current_rx_seq          = stats.mavStats.current_rx_seq;
+            tm.current_tx_seq          = stats.mavStats.current_tx_seq;
+            tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count;
+            tm.packet_rx_drop_count    = stats.mavStats.packet_rx_drop_count;
+
+            mavlink_msg_mavlink_stats_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                                Config::Radio::MAV_COMPONENT_ID,
+                                                &msg, &tm);
+            return true;
+        }
 
-    // TODO: Handle buttons
+        case MAV_GSE_ID:
+        {
+            mavlink_gse_tm_t tm = {0};
+
+            tm.timestamp       = TimestampTimer::getTimestamp();
+            tm.loadcell_rocket = 69;
+            tm.loadcell_vessel = 420;
+            // TODO(davide.mor): Add the rest of these
+
+            mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                      Config::Radio::MAV_COMPONENT_ID, &msg,
+                                      &tm);
+            return true;
+        }
+
+        case MAV_MOTOR_ID:
+        {
+            mavlink_motor_tm_t tm;
+
+            tm.timestamp            = TimestampTimer::getTimestamp();
+            tm.top_tank_pressure    = 69;
+            tm.bottom_tank_pressure = 420;
+
+            mavlink_msg_motor_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+                                        Config::Radio::MAV_COMPONENT_ID, &msg,
+                                        &tm);
+            return true;
+        }
+
+        default:
+        {
+            return false;
+        }
+    }
 }
 
-void Radio::sendFakeGseTm()
+void Radio::handleConrigState(const mavlink_message_t& msg)
 {
-    mavlink_gse_tm_t tm = {0};
-    mavlink_message_t msg;
+    // Acknowledge the state
+    sendAck(msg);
 
-    tm.timestamp = 69;
+    // Flush all pending packets
+    flushPackets();
 
-    mavlink_msg_gse_tm_encode(Config::Radio::MAV_SYSTEM_ID,
-                              Config::Radio::MAV_COMPONENT_ID, &msg, &tm);
+    // Send GSE and motor telemetry
+    mavlink_message_t tm;
+    packSystemTm(MAV_GSE_ID, tm);
+    enqueuePacket(tm);
+    packSystemTm(MAV_MOTOR_ID, tm);
+    enqueuePacket(tm);
 
-    queuePacket(msg);
+    // TODO: Handle buttons
 }
diff --git a/src/boards/RIGv2/Radio/Radio.h b/src/boards/RIGv2/Radio/Radio.h
index 7880b6a4863148c8ce3c8671f14ff0fd000244a9..0a9090eec94761d4d7da1ae2005ae04728b0b758 100644
--- a/src/boards/RIGv2/Radio/Radio.h
+++ b/src/boards/RIGv2/Radio/Radio.h
@@ -46,20 +46,22 @@ public:
 
     void stop();
 
+    bool isStarted();
+
     Boardcore::MavlinkStatus getMavStatus();
 
 private:
     void sendAck(const mavlink_message_t& msg);
     void sendNack(const mavlink_message_t& msg);
 
-    void queuePacket(const mavlink_message_t &msg);
+    void enqueuePacket(const mavlink_message_t &msg);
     void flushPackets();
 
     void handleMessage(const mavlink_message_t& msg);
     void handleCommand(const mavlink_message_t& msg);
     void handleConrigState(const mavlink_message_t& msg);
-
-    void sendFakeGseTm();
+    
+    bool packSystemTm(uint8_t tmId, mavlink_message_t& msg);
 
     Boardcore::Logger& sdLogger   = Boardcore::Logger::getInstance();
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("radio");
@@ -68,6 +70,7 @@ private:
                               Config::Radio::CIRCULAR_BUFFER_SIZE>
         queuedPackets;
 
+    std::atomic<bool> started{false};
     std::unique_ptr<Boardcore::SX1278Lora> radio;
     std::unique_ptr<MavDriver> mavDriver;
 };
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 0415649cdfc09d309a328d7f48b99df274f41e9a..4318603df29f517a61fe4b9f3c77a9cab53d42c2 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -31,16 +31,29 @@ using namespace Boardcore;
 using namespace miosix;
 using namespace RIGv2;
 
+bool Sensors::isStarted() { return started; }
+
 bool Sensors::start()
 {
     SensorManager::SensorMap_t map;
     adc1Init(map);
 
     manager = std::make_unique<SensorManager>(map, &scheduler);
-    return manager->start();
+    if (!manager->start())
+    {
+        LOG_ERR(logger, "Failed to start SensorManager");
+        return false;
+    }
+
+    started = true;
+    return true;
 }
 
-void Sensors::stop() { manager->stop(); }
+void Sensors::stop()
+{
+    manager->stop();
+    started = false;
+}
 
 ADS131M08Data Sensors::getADC1LastSample()
 {
diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h
index ec32a4674ae72d9c391a8b996afadf3431d2e494..14eeef1914e75006e986c78b44beeb80aef22db4 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -26,6 +26,7 @@
 #include <sensors/SensorManager.h>
 
 #include <memory>
+#include <atomic>
 #include <utils/ModuleManager/ModuleManager.hpp>
 
 namespace RIGv2
@@ -40,6 +41,8 @@ public:
 
     void stop();
 
+    bool isStarted();
+
     Boardcore::ADS131M08Data getADC1LastSample();
 
 private:
@@ -50,6 +53,7 @@ private:
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
     Boardcore::TaskScheduler &scheduler;
 
+    std::atomic<bool> started{false};
     std::unique_ptr<Boardcore::ADS131M08> adc1;
     std::unique_ptr<Boardcore::SensorManager> manager;
 };