diff --git a/src/boards/Parafoil/Main/Radio.cpp b/src/boards/Parafoil/Main/Radio.cpp
index bb032df5259060abb99558ce33ada4d24eb70d31..6d458e67ce546299b1ebb0153c4cb82eca2e16ed 100644
--- a/src/boards/Parafoil/Main/Radio.cpp
+++ b/src/boards/Parafoil/Main/Radio.cpp
@@ -103,10 +103,10 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
     // handle TC
     switch (msg.msgid)
     {
-        case MAVLINK_MSG_ID_NOARG_TC:  // basic command
+        case MAVLINK_MSG_ID_COMMAND_TC:  // basic command
         {
             LOG_DEBUG(logger, "Received NOARG command");
-            uint8_t commandId = mavlink_msg_noarg_tc_get_command_id(&msg);
+            uint8_t commandId = mavlink_msg_command_tc_get_command_id(&msg);
 
             // search for the corresponding event and post it
             auto it = tcMap.find(commandId);
@@ -117,52 +117,59 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
 
             switch (commandId)
             {
-                case MAV_CMD_BOARD_RESET:
+                case MAV_CMD_FORCE_REBOOT:
                     SDlogger->stop();
                     LOG_INFO(logger, "Received command BOARD_RESET");
                     miosix::reboot();
                     break;
                 case MAV_CMD_CLOSE_LOG:
                     SDlogger->stop();
-                    sendTelemetry(MAV_LOGGER_TM_ID);
+                    sendSystemTelemetry(MAV_LOGGER_ID);
                     LOG_INFO(logger, "Received command CLOSE_LOG");
                     break;
                 case MAV_CMD_START_LOGGING:
                     ParafoilTest::getInstance().SDlogger->start();
-                    sendTelemetry(MAV_LOGGER_TM_ID);
+                    sendSystemTelemetry(MAV_LOGGER_ID);
                     LOG_INFO(logger, "Received command START_LOG");
                     break;
                 case MAV_CMD_TEST_MODE:
                     // I use the test mode to apply the sequence
                     ParafoilTest::getInstance().wingController->start();
                     break;
-                case MAV_CMD_DPL_RESET_SERVO:
+                case MAV_CMD_FORCE_LANDING:
                     // I reset the servo position
                     ParafoilTest::getInstance().wingController->reset();
                     break;
-                case MAV_CMD_CALIBRATE_SENSORS:
-                    // I calibrate the sensors inside Sensors.h
-                    ParafoilTest::getInstance().sensors->calibrate();
-                    break;
                 default:
                     break;
             }
             break;
         }
-        case MAVLINK_MSG_ID_TELEMETRY_REQUEST_TC:  // tm request
+        case MAVLINK_MSG_ID_SYSTEM_TELEMETRY_REQUEST_TC:  // tm request
         {
-            uint8_t tmId = mavlink_msg_telemetry_request_tc_get_board_id(&msg);
+            uint8_t tmId =
+                mavlink_msg_system_telemetry_request_tc_get_tm_id(&msg);
             LOG_DEBUG(logger, "Received TM request : id = {:d}", tmId);
 
             // send corresponding telemetry or NACK
-            sendTelemetry(tmId);
+            sendSystemTelemetry(tmId);
+
+            break;
+        }
+        case MAVLINK_MSG_ID_SENSOR_TELEMETRY_REQUEST_TC:
+        {
+            uint8_t tmId =
+                mavlink_msg_sensor_telemetry_request_tc_get_sensor_id(&msg);
+
+            // Send corresponding telemetry or NACK
+            sendSensorTelemetry(tmId);
 
             break;
         }
-        case MAVLINK_MSG_ID_SET_AEROBRAKE_ANGLE_TC:
+        case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC:
         {
             uint8_t algorithmId =
-                mavlink_msg_set_aerobrake_angle_tc_get_angle(&msg);
+                mavlink_msg_set_servo_angle_tc_get_angle(&msg);
 
             // Set the algorithm (invalid cases are checked inside the method)
             ParafoilTest::getInstance().wingController->selectAlgorithm(
@@ -185,8 +192,8 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
 
             // post given event on given topic
             EventBroker::getInstance().post(
-                {mavlink_msg_raw_event_tc_get_Event_id(&msg)},
-                mavlink_msg_raw_event_tc_get_Topic_id(&msg));
+                {mavlink_msg_raw_event_tc_get_event_id(&msg)},
+                mavlink_msg_raw_event_tc_get_topic_id(&msg));
             break;
         }
         case MAVLINK_MSG_ID_PING_TC:
@@ -205,30 +212,38 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
     sendAck(msg);
 }
 
-bool Radio::sendTelemetry(const uint8_t tm_id)
+bool Radio::sendSystemTelemetry(const uint8_t tm_id)
 {
     // Enqueue the message
     bool result =
-        mav_driver->enqueueMsg(TMRepository::getInstance().packTM(tm_id));
+        mav_driver->enqueueMsg(TMRepository::getInstance().packSystemTM(tm_id));
     // TODO log the operation
     return result;
 }
 
+bool Radio::sendSensorTelemetry(const uint8_t tm_id)
+{
+    // Enqueue the message
+    bool result =
+        mav_driver->enqueueMsg(TMRepository::getInstance().packSensorTM(tm_id));
+    return result;
+}
+
 void Radio::sendHRTelemetry()
 {
     // sendTelemetry(MAV_HR_TM_ID);
     // TEST ONLY
-    sendTelemetry(MAV_SENSORS_TM_ID);
+    sendSystemTelemetry(MAV_FLIGHT_ID);
 }
 
 void Radio::sendLRTelemetry()
 {
     // I send this telemetry if and only if the status is
     // in low rate telemetry
-    sendTelemetry(MAV_LR_TM_ID);
+    sendSystemTelemetry(MAV_FLIGHT_STATS_ID);
 }
 
-void Radio::sendSDLogTelemetry() { sendTelemetry(MAV_LOGGER_TM_ID); }
+void Radio::sendSDLogTelemetry() { sendSystemTelemetry(MAV_LOGGER_ID); }
 
 void Radio::sendAck(const mavlink_message_t& msg)
 {
@@ -313,7 +328,7 @@ void Radio::init()
 
     // Register the LR and HR tasks in the scheduler
     scheduler->addTask(HRfunction, HR_GROUND_UPDATE_PERIOD, RADIO_HR_ID);
-    scheduler->addTask(LRfunction, LR_UPDATE_PERIOD, RADIO_LR_ID);
+    // scheduler->addTask(LRfunction, LR_UPDATE_PERIOD, RADIO_LR_ID);
     scheduler->addTask(SDfunction, SD_UPDATE_PERIOD, SD_UPDATE_ID);
 
     // Set the frame receive callback
diff --git a/src/boards/Parafoil/Main/Radio.h b/src/boards/Parafoil/Main/Radio.h
index a63bcba5a3d198643c11fbd1fab851ffa759580f..e1c6dbda2269d289c7026ebde97a7f3f220fddf7 100644
--- a/src/boards/Parafoil/Main/Radio.h
+++ b/src/boards/Parafoil/Main/Radio.h
@@ -70,13 +70,23 @@ public:
 
     /**
      * @brief This method is used to add in send queue
-     * the requested message. It is necessary to call the
+     * the requested system message. It is necessary to call the
      * TMrepository to process the actual packet.
      *
      * @param tm_id The requested message id
      * @return boolean that indicates the operation's result
      */
-    bool sendTelemetry(const uint8_t tm_id);
+    bool sendSystemTelemetry(const uint8_t tm_id);
+
+    /**
+     * @brief This method is used to add in send queue
+     * the requested sensor message. It is necessary to call the
+     * TMrepository to process the actual packet.
+     *
+     * @param tm_id The requested message id
+     * @return boolean that indicates the operation's result
+     */
+    bool sendSensorTelemetry(const uint8_t tm_id);
 
     /**
      * @brief Method automatically called by the task
@@ -120,31 +130,20 @@ private:
         {MAV_CMD_ARM, EV_TC_ARM},
         {MAV_CMD_DISARM, EV_TC_DISARM},
 
-        {MAV_CMD_FORCE_INIT, EV_TC_FORCE_INIT},
         {MAV_CMD_FORCE_LAUNCH, EV_TC_LAUNCH},
+        {MAV_CMD_FORCE_LANDING, EV_TC_LAND},
 
-        {MAV_CMD_NOSECONE_OPEN, EV_TC_NC_OPEN},
-        {MAV_CMD_DPL_RESET_SERVO, EV_TC_DPL_RESET_SERVO},
-        {MAV_CMD_DPL_WIGGLE_SERVO, EV_TC_DPL_WIGGLE_SERVO},
-        {MAV_CMD_CUT_DROGUE, EV_TC_CUT_DROGUE},
-
-        {MAV_CMD_ARB_RESET_SERVO, EV_TC_ABK_RESET_SERVO},
-        {MAV_CMD_ARB_WIGGLE_SERVO, EV_TC_ABK_WIGGLE_SERVO},
-        {MAV_CMD_DISABLE_AEROBRAKES, EV_TC_ABK_DISABLE},
-        {MAV_CMD_TEST_AEROBRAKES, EV_TC_TEST_ABK},
-
-        {MAV_CMD_CALIBRATE_ALGOS, EV_TC_CALIBRATE_ALGOS},
-        {MAV_CMD_CALIBRATE_SENSORS, EV_TC_CALIBRATE_SENSORS},
-
-        {MAV_CMD_SERIAL_TM, EV_TC_SERIAL_TM},
-        {MAV_CMD_RADIO_TM, EV_TC_RADIO_TM},
+        {MAV_CMD_FORCE_EXPULSION, EV_TC_NC_OPEN},
+        {MAV_CMD_FORCE_MAIN, EV_TC_CUT_DROGUE},
 
         {MAV_CMD_START_LOGGING, EV_TC_START_LOG},
         {MAV_CMD_CLOSE_LOG, EV_TC_CLOSE_LOG},
 
+        {MAV_CMD_FORCE_REBOOT, EV_TC_RESET_BOARD},
         {MAV_CMD_TEST_MODE, EV_TC_TEST_MODE},
-        {MAV_CMD_BOARD_RESET, EV_TC_RESET_BOARD},
-        {MAV_CMD_END_MISSION, EV_TC_END_MISSION}};
+
+        {MAV_CMD_START_RECORDING, EV_TC_START_RECORDING},
+        {MAV_CMD_STOP_RECORDING, EV_TC_STOP_RECORDING}};
 
     /**
      * @brief The mavlink driver
diff --git a/src/boards/Parafoil/Main/Sensors.cpp b/src/boards/Parafoil/Main/Sensors.cpp
index 68c1d220fb6ce9b0d75b3e6339165f7354be474b..d9dbc7dd07358049f0cee7786cfe53ecd220e722 100644
--- a/src/boards/Parafoil/Main/Sensors.cpp
+++ b/src/boards/Parafoil/Main/Sensors.cpp
@@ -67,9 +67,6 @@ void Sensors::MPU9250Callback()
     logger->log(imu_mpu9250->getLastSample());
     LOG_DEBUG(log, "{:.2f} {:.2f} {:.2f}", d.accelerationX, d.accelerationY,
               d.accelerationZ);
-
-    // Update the radio repository
-    TMRepository::getInstance().update(d);
 }
 
 void Sensors::UbloxGPSinit()
@@ -97,9 +94,6 @@ void Sensors::UbloxGPSCallback()
     {
         LOG_DEBUG(log, "{:.2f} {:.2f}", d.latitude, d.longitude);
     }
-
-    // Update the radio repository
-    TMRepository::getInstance().update(d);
 }
 
 void Sensors::BME280init()
@@ -196,7 +190,6 @@ void Sensors::BME280Callback()
         // Update the radio repository
         d.pressure    = pressureMean.getStats().mean;
         d.temperature = temperatureMean.getStats().mean;
-        TMRepository::getInstance().update(d);
 
         // Decide what the wing should do
         if (height > WING_ALGORITHM_ARM_ALTITUDE && !procedureArm)
diff --git a/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h b/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h
index 2ddad64c8513396e39e16c0cfbf78ebca216ea44..6b768ab2f5f17acebfa1b065f65afabc953e1a80 100644
--- a/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h
+++ b/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h
@@ -27,7 +27,7 @@
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-align"
 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
-#include <mavlink_lib/lynx/mavlink.h>
+#include <mavlink_lib/pyxis/mavlink.h>
 #pragma GCC diagnostic pop
 
 #include <Parafoil/Configs/MavlinkConfig.h>
diff --git a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp
index 5fbc80b73580c10d4b6aa535805f5b473764b973..43963cc65c1d752e5f9666ccb2c2268bee92c157 100644
--- a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp
+++ b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp
@@ -22,14 +22,18 @@
 
 #include <Parafoil/ParafoilTest.h>
 #include <Parafoil/TelemetriesTelecommands/TMRepository.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <Eigen/Core>
 
 using namespace Boardcore;
+using namespace Eigen;
 
 namespace Parafoil
 {
 
-mavlink_message_t TMRepository::packTM(uint8_t req_tm, uint8_t sys_id,
-                                       uint8_t comp_id)
+mavlink_message_t TMRepository::packSystemTM(uint8_t req_tm, uint8_t sys_id,
+                                             uint8_t comp_id)
 {
     mavlink_message_t m;
     mavlink_nack_tm_t nack_tm;
@@ -38,73 +42,120 @@ mavlink_message_t TMRepository::packTM(uint8_t req_tm, uint8_t sys_id,
 
     switch (req_tm)
     {
-        case MavTMList::MAV_SENSORS_TM_ID:
+        case SystemTMList::MAV_LOGGER_ID:
         {
-            tm_repository.sensors_tm.timestamp = miosix::getTick();
-            mavlink_msg_sensors_tm_encode(sys_id, comp_id, &m,
-                                          &(tm_repository.sensors_tm));
-            break;
-        }
-        case MavTMList::MAV_LOGGER_TM_ID:
-        {
-            tm_repository.logger_tm.timestamp = miosix::getTick();
+            tmRepository.loggerTm.timestamp = miosix::getTick();
 
             // Get the logger stats
             Boardcore::LoggerStats stats =
                 ParafoilTest::getInstance().SDlogger->getStats();
 
-            // First i update the logger_tm
-            tm_repository.logger_tm.statLogNumber =
+            // First i update the loggerTm
+            tmRepository.loggerTm.log_number =
                 ParafoilTest::getInstance().SDlogger->getCurrentLogNumber();
 
-            tm_repository.logger_tm.statBufferFilled    = stats.buffersFilled;
-            tm_repository.logger_tm.statBufferWritten   = stats.buffersWritten;
-            tm_repository.logger_tm.statDroppedSamples  = stats.droppedSamples;
-            tm_repository.logger_tm.statMaxWriteTime    = stats.maxWriteTime;
-            tm_repository.logger_tm.statQueuedSamples   = stats.queuedSamples;
-            tm_repository.logger_tm.statTooLargeSamples = stats.tooLargeSamples;
-            tm_repository.logger_tm.statWriteFailed     = stats.writesFailed;
-            tm_repository.logger_tm.statWriteTime       = stats.maxWriteTime;
-            tm_repository.logger_tm.statWriteError      = stats.lastWriteError;
+            tmRepository.loggerTm.filled_buffers    = stats.buffersFilled;
+            tmRepository.loggerTm.written_buffers   = stats.buffersWritten;
+            tmRepository.loggerTm.sdropped_samples  = stats.droppedSamples;
+            tmRepository.loggerTm.max_write_time    = stats.maxWriteTime;
+            tmRepository.loggerTm.queued_samples    = stats.queuedSamples;
+            tmRepository.loggerTm.too_large_samples = stats.tooLargeSamples;
+            tmRepository.loggerTm.failed_writes     = stats.writesFailed;
+            tmRepository.loggerTm.max_write_time    = stats.maxWriteTime;
+            tmRepository.loggerTm.error_writes      = stats.lastWriteError;
 
             mavlink_msg_logger_tm_encode(sys_id, comp_id, &m,
-                                         &(tm_repository.logger_tm));
+                                         &(tmRepository.loggerTm));
+            break;
+        }
+        case SystemTMList::MAV_FLIGHT_ID:
+        {
+            // I have to send the whole flight tm
+            tmRepository.flightTm.timestamp = miosix::getTick();
+
+            // Get the pressure
+            tmRepository.flightTm.pressure_digi =
+                ParafoilTest::getInstance()
+                    .sensors->getBME280LastSample()
+                    .pressure;
+
+            // Get the IMU data
+            MPU9250Data imu =
+                ParafoilTest::getInstance().sensors->getMPU9250LastSample();
+
+            tmRepository.flightTm.acc_x  = imu.accelerationX;
+            tmRepository.flightTm.acc_y  = imu.accelerationY;
+            tmRepository.flightTm.acc_z  = imu.accelerationZ;
+            tmRepository.flightTm.gyro_x = imu.angularVelocityX;
+            tmRepository.flightTm.gyro_y = imu.angularVelocityY;
+            tmRepository.flightTm.gyro_z = imu.angularVelocityZ;
+            tmRepository.flightTm.mag_x  = imu.magneticFieldX;
+            tmRepository.flightTm.mag_y  = imu.magneticFieldY;
+            tmRepository.flightTm.mag_z  = imu.magneticFieldZ;
+
+            // Get the GPS data
+            UBXGPSData gps =
+                ParafoilTest::getInstance().sensors->getGPSLastSample();
+
+            tmRepository.flightTm.gps_fix = gps.fix;
+            tmRepository.flightTm.gps_lat = gps.latitude;
+            tmRepository.flightTm.gps_lon = gps.longitude;
+            tmRepository.flightTm.gps_alt = gps.height;
+
+            // Get the NAS data
+            NASState state =
+                ParafoilTest::getInstance().algorithms->getNASLastSample();
+
+            tmRepository.flightTm.nas_x  = state.n;
+            tmRepository.flightTm.nas_y  = state.e;
+            tmRepository.flightTm.nas_z  = state.d;
+            tmRepository.flightTm.nas_vx = state.vn;
+            tmRepository.flightTm.nas_vy = state.ve;
+            tmRepository.flightTm.nas_vz = state.vd;
+
+            // TODO discuss about quaternion to euler computation in this
+            // instance
+
+            tmRepository.flightTm.nas_bias0 = state.bx;
+            tmRepository.flightTm.nas_bias1 = state.by;
+            tmRepository.flightTm.nas_bias2 = state.bz;
+
             break;
         }
         /*case MavTMList::MAV_SYS_TM_ID:
-            tm_repository.sys_tm.timestamp = miosix::getTick();
+            tmRepository.sys_tm.timestamp = miosix::getTick();
             mavlink_msg_sys_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.sys_tm));
+                                    &(tmRepository.sys_tm));
             break;
         case MavTMList::MAV_FMM_TM_ID:
-            tm_repository.fmm_tm.timestamp = miosix::getTick();
+            tmRepository.fmm_tm.timestamp = miosix::getTick();
             mavlink_msg_fmm_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.fmm_tm));
+                                    &(tmRepository.fmm_tm));
             break;
         case MavTMList::MAV_TMTC_TM_ID:
-            tm_repository.tmtc_tm.timestamp = miosix::getTick();
+            tmRepository.tmtc_tm.timestamp = miosix::getTick();
             mavlink_msg_tmtc_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.tmtc_tm));
+                                    &(tmRepository.tmtc_tm));
             break;
         case MavTMList::MAV_TASK_STATS_TM_ID:
-            tm_repository.task_stats_tm.timestamp = miosix::getTick();
+            tmRepository.task_stats_tm.timestamp = miosix::getTick();
             mavlink_msg_task_stats_tm_encode(sys_id, comp_id, &m,
-                                            &(tm_repository.task_stats_tm));
+                                            &(tmRepository.task_stats_tm));
             break;
         case MavTMList::MAV_GPS_TM_ID:
-            tm_repository.gps_tm.timestamp = miosix::getTick();
+            tmRepository.gps_tm.timestamp = miosix::getTick();
             mavlink_msg_gps_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.gps_tm));
+                                    &(tmRepository.gps_tm));
             break;
         case MavTMList::MAV_HR_TM_ID:
-            tm_repository.hr_tm.timestamp = miosix::getTick();
+            tmRepository.hr_tm.timestamp = miosix::getTick();
             mavlink_msg_hr_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.hr_tm));
+                                    &(tmRepository.hr_tm));
             break;
         case MavTMList::MAV_LR_TM_ID:
-            //tm_repository.tm_repository.lr_tm.timestamp = miosix::getTick();
+            //tmRepository.tmRepository.lr_tm.timestamp = miosix::getTick();
             mavlink_msg_lr_tm_encode(sys_id, comp_id, &m,
-                                    &(tm_repository.lr_tm));
+                                    &(tmRepository.lr_tm));
             break;*/
         default:
         {
@@ -118,45 +169,15 @@ mavlink_message_t TMRepository::packTM(uint8_t req_tm, uint8_t sys_id,
     return m;
 }
 
-// Implement all the update functions
-void TMRepository::update(MPU9250Data data)
+mavlink_message_t TMRepository::packSensorTM(uint8_t req_tm, uint8_t sys_id,
+                                             uint8_t comp_id)
 {
-    // Pause the kernel to avoid interructions during this fast operation
-    miosix::PauseKernelLock kLock;
-    // Update only the sensors message TODO update all the things
-    tm_repository.sensors_tm.bmx160_acc_x = data.accelerationX;
-    tm_repository.sensors_tm.bmx160_acc_y = data.accelerationY;
-    tm_repository.sensors_tm.bmx160_acc_z = data.accelerationZ;
-
-    tm_repository.sensors_tm.bmx160_gyro_x = data.angularVelocityX;
-    tm_repository.sensors_tm.bmx160_gyro_y = data.angularVelocityY;
-    tm_repository.sensors_tm.bmx160_gyro_z = data.angularVelocityZ;
-
-    tm_repository.sensors_tm.bmx160_mag_x = data.magneticFieldX;
-    tm_repository.sensors_tm.bmx160_mag_y = data.magneticFieldY;
-    tm_repository.sensors_tm.bmx160_mag_z = data.magneticFieldZ;
-
-    tm_repository.sensors_tm.bmx160_temp = data.temperature;
-}
+    mavlink_message_t m;
+    mavlink_nack_tm_t nack_tm;
 
-void TMRepository::update(UBXGPSData data)
-{
-    // Pause the kernel to avoid interructions during this fast operation
     miosix::PauseKernelLock kLock;
-    // Update only the sensors message TODO update all the things
-    tm_repository.sensors_tm.gps_alt = data.height;
-    tm_repository.sensors_tm.gps_fix = data.fix;
-    tm_repository.sensors_tm.gps_lon = data.longitude;
-    tm_repository.sensors_tm.gps_lat = data.latitude;
-}
 
-void TMRepository::update(BME280Data data)
-{
-    // Pause the kernel to avoid interructions during this fast operation
-    miosix::PauseKernelLock kLock;
-    // Update only the sensors message TODO update all the things
-    tm_repository.sensors_tm.ms5803_press = data.pressure;
-    tm_repository.sensors_tm.ms5803_temp  = data.temperature;
+    return m;
 }
 
 }  // namespace Parafoil
diff --git a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h
index a8a872e22840d7fc6cc4f3007b84a500b8716ca8..8e8e8c8989c4a2632baa5a809aff0898d832f6fa 100644
--- a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h
+++ b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h
@@ -46,7 +46,7 @@ class TMRepository : public Boardcore::Singleton<TMRepository>
 
 public:
     /**
-     * @brief Retrieve a telemetry message in packed form.
+     * @brief Retrieve a system telemetry message in packed form.
      *
      * @param req_tm    required telemetry
      * @param sys_id    system id to pack it with
@@ -54,15 +54,22 @@ public:
      * @return          packed mavlink struct of that telemetry or a NACK_TM if
      *                  the telemetry id was not found.
      */
-    mavlink_message_t packTM(uint8_t req_tm, uint8_t sys_id = TMTC_MAV_SYSID,
-                             uint8_t comp_id = TMTC_MAV_COMPID);
+    mavlink_message_t packSystemTM(uint8_t req_tm,
+                                   uint8_t sys_id  = TMTC_MAV_SYSID,
+                                   uint8_t comp_id = TMTC_MAV_COMPID);
 
     /**
-     * @brief Update functions
+     * @brief Retrieve a sensor telemetry message in packed form.
+     *
+     * @param req_tm    required telemetry
+     * @param sys_id    system id to pack it with
+     * @param comp_id   component id to pack it with
+     * @return          packed mavlink struct of that telemetry or a NACK_TM if
+     *                  the telemetry id was not found.
      */
-    void update(Boardcore::MPU9250Data data);
-    void update(Boardcore::UBXGPSData data);
-    void update(Boardcore::BME280Data data);
+    mavlink_message_t packSensorTM(uint8_t req_tm,
+                                   uint8_t sys_id  = TMTC_MAV_SYSID,
+                                   uint8_t comp_id = TMTC_MAV_COMPID);
 
 private:
     /**
@@ -70,29 +77,29 @@ private:
      */
     struct TmRepository_t
     {
-        mavlink_sys_tm_t sys_tm;
-        mavlink_pin_obs_tm_t pin_obs_tm;
-        mavlink_logger_tm_t logger_tm;
-        mavlink_fmm_tm_t fmm_tm;
-        mavlink_tmtc_tm_t tmtc_tm;
-        mavlink_task_stats_tm_t task_stats_tm;
-        mavlink_dpl_tm_t dpl_tm;
-        mavlink_ada_tm_t ada_tm;
-        mavlink_abk_tm_t abk_tm;
-        mavlink_nas_tm_t nas_tm;
-
-        mavlink_ms5803_tm_t digital_baro_tm;
-        mavlink_bmx160_tm_t bmx_tm;
-        mavlink_lis3mdl_tm_t lis3mdl_tm;
-        mavlink_adc_tm_t adc_tm;
-        mavlink_gps_tm_t gps_tm;
+        // System telemetries
+        mavlink_sys_tm_t sysTm;
+        mavlink_fsm_tm_t fsmTm;
+        mavlink_pin_obs_tm_t pinObsTm;
+        mavlink_logger_tm_t loggerTm;
+        mavlink_mavlink_stats_tm_t mavlinkStatsTm;
+        mavlink_task_stats_tm_t taskStatsTm;
+        mavlink_dpl_tm_t dplTm;
+        mavlink_ada_tm_t adaTm;
+        mavlink_nas_tm_t nasTm;
+        mavlink_can_tm_t canTm;
+        mavlink_payload_flight_tm_t flightTm;
+        mavlink_payload_stats_tm_t stastTm;
+        mavlink_sensor_state_tm_t sensorsStateTm;
 
-        mavlink_hr_tm_t hr_tm;
-        mavlink_lr_tm_t lr_tm;
-        mavlink_windtunnel_tm_t wind_tm;
-        mavlink_sensors_tm_t sensors_tm;
-        mavlink_test_tm_t test_tm;
-    } tm_repository;
+        // Sensors telemetries
+        mavlink_gps_tm_t gpsTm;
+        mavlink_imu_tm_t imuTm;
+        mavlink_adc_tm_t adcTm;
+        mavlink_baro_tm_t barometerTm;
+        mavlink_temp_tm_t temperatureTm;
+        mavlink_attitude_tm_t attitudeTm;
+    } tmRepository;
 
     /**
      * @brief Logger