diff --git a/Modules/CommandPad/commandpad.cpp b/Modules/CommandPad/commandpad.cpp
index 3ad65a420cc8ce64f5133c933c54926721369b41..5b7fadd13b64522528524928f4f2ed3cce17567b 100644
--- a/Modules/CommandPad/commandpad.cpp
+++ b/Modules/CommandPad/commandpad.cpp
@@ -9,12 +9,12 @@ const QMap<QString, uint8_t> CommandPad::systemTMList {
     { "MAV_LOGGER_ID", 4 },
     { "MAV_MAVLINK_STATS", 5 },
     { "MAV_TASK_STATS_ID", 6 },
-    { "MAV_DPL_ID", 7 },
+    // { "MAV_DPL_ID", 7 },
     { "MAV_ADA_ID", 8 },
     { "MAV_NAS_ID", 9 },
     { "MAV_CAN_ID", 10 },
     { "MAV_FLIGHT_ID", 11 },
-    { "MAV_FLIGHT_STATS_ID", 12 },
+    { "MAV_STATS_ID", 12 },
     { "MAV_SENSORS_STATE_ID", 13 }
 };
 
@@ -25,13 +25,14 @@ const QMap<QString, uint8_t> CommandPad::sensorsTMList = {
     { "MAV_MPU9250_ID", 4 },
     { "MAV_ADS_ID", 5 },
     { "MAV_MS5803_ID", 6 },
-    { "MAV_CURRENT_SENSE_ID", 7 },
-    { "MAV_LIS3MDL_ID", 8 },
-    { "MAV_DPL_PRESS_ID", 9 },
-    { "MAV_STATIC_PRESS_ID", 10 },
-    { "MAV_PITOT_PRESS_ID", 11 },
-    { "MAV_BATTERY_VOLTAGE_ID", 12 },
-    { "MAV_STRAIN_GAUGE_ID", 13 },
+    { "MAV_BME280_ID", 7 },
+    { "MAV_CURRENT_SENSE_ID", 8 },
+    { "MAV_LIS3MDL_ID", 9 },
+    { "MAV_DPL_PRESS_ID", 10 },
+    { "MAV_STATIC_PRESS_ID", 11 },
+    { "MAV_PITOT_PRESS_ID", 12 },
+    { "MAV_BATTERY_VOLTAGE_ID", 13 },
+    { "MAV_STRAIN_GAUGE_ID", 14 },
 };
 
 const QMap<QString, uint8_t> CommandPad::mavCommandList = {
@@ -142,12 +143,15 @@ void CommandPad::setupUi() {
     _CMD(command, "COMMAND_TC")
     _CMD_COMBO_PROP(command, command_id, mavCommandList)
     _CMD_END
-    _CMD(systemTelemetry, "SYSTEM_TELEMETRY_REQUEST_TC")
+    _CMD(systemTelemetry, "SYSTEM_TM_REQUEST_TC")
     _CMD_COMBO_PROP(systemTelemetry, tm_id, systemTMList)
     _CMD_END
-    _CMD(sensorTelemetry, "SENSOR_TELEMETRY_REQUEST_TC")
+    _CMD(sensorTelemetry, "SENSOR_TM_REQUEST_TC")
     _CMD_COMBO_PROP(sensorTelemetry, sensor_id, sensorsTMList)
     _CMD_END
+    _CMD(servoTelemetry, "SERVO_TM_REQUEST_TC")
+    _CMD_COMBO_PROP(servoTelemetry, servo_id, servosList)
+    _CMD_END
     _CMD(setServoAngle, "SET_SERVO_ANGLE_TC")
     _CMD_COMBO_PROP(setServoAngle, servo_id, servosList)
     _CMD_FLOAT_PROP(setServoAngle, angle)
@@ -180,6 +184,13 @@ void CommandPad::setupUi() {
     _CMD_UINT8_PROP(rawEvent, topic_id)
     _CMD_UINT8_PROP(rawEvent, event_id)
     _CMD_END
+    _CMD(targetCoordinates, "SET_TARGET_COORDINATES_TC")
+    _CMD_FLOAT_PROP(targetCoordinates, latitude)
+    _CMD_FLOAT_PROP(targetCoordinates, longitude)
+    _CMD_END
+    _CMD(algorithm, "SET_ALGORITHM_TC")
+    _CMD_UINT8_PROP(algorithm, algorithm_number)
+    _CMD_END
 
     QPushButton* send = new QPushButton(tr("Send"));
     connect(send, &QPushButton::clicked, [ = ]() {
@@ -188,12 +199,15 @@ void CommandPad::setupUi() {
         _SEND(command, "COMMAND_TC")
         _SEND_COMBO_PROP(command, command_id, mavCommandList)
         _SEND_END
-        _SEND(systemTelemetry, "SYSTEM_TELEMETRY_REQUEST_TC")
+        _SEND(systemTelemetry, "SYSTEM_TM_REQUEST_TC")
         _SEND_COMBO_PROP(systemTelemetry, tm_id, systemTMList)
         _SEND_END
-        _SEND(sensorTelemetry, "SENSOR_TELEMETRY_REQUEST_TC")
+        _SEND(sensorTelemetry, "SENSOR_TM_REQUEST_TC")
         _SEND_COMBO_PROP(sensorTelemetry, sensor_id, sensorsTMList)
         _SEND_END
+        _SEND(servoTelemetry, "SERVO_TM_REQUEST_TC")
+        _SEND_COMBO_PROP(servoTelemetry, servo_id, servosList)
+        _SEND_END
         _SEND(setServoAngle, "SET_SERVO_ANGLE_TC")
         _SEND_COMBO_PROP(setServoAngle, servo_id, servosList)
         _SEND_FLOAT_PROP(setServoAngle, angle)
@@ -226,6 +240,13 @@ void CommandPad::setupUi() {
         _SEND_UINT8_PROP(rawEvent, topic_id)
         _SEND_UINT8_PROP(rawEvent, event_id)
         _SEND_END
+        _SEND(targetCoordinates, "SET_TARGET_COORDINATES_TC")
+        _SEND_FLOAT_PROP(targetCoordinates, latitude)
+        _SEND_FLOAT_PROP(targetCoordinates, longitude)
+        _SEND_END
+        _SEND(algorithm, "SET_ALGORITHM_TC")
+        _SEND_UINT8_PROP(algorithm, algorithm_number)
+        _SEND_END
     });
 
     QVBoxLayout* layout = new QVBoxLayout;
diff --git a/Modules/Mavlink/mavlinkcommandadapter.cpp b/Modules/Mavlink/mavlinkcommandadapter.cpp
index 2ec462fcd0b228cb37c0c7a9214459e20622b533..2b44645e3b3319b82d4c3de71dcb4e71956b8f18 100644
--- a/Modules/Mavlink/mavlinkcommandadapter.cpp
+++ b/Modules/Mavlink/mavlinkcommandadapter.cpp
@@ -22,11 +22,14 @@ bool MavlinkCommandAdapter::encodeCommand(const ModuleMessage& msg, mavlink_mess
     } else if(messageName == "COMMAND_TC") {
         mavlink_msg_command_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("command_id").getUInteger(0));
         return true;
-    } else if(messageName == "SYSTEM_TELEMETRY_REQUEST_TC") {
-        mavlink_msg_system_telemetry_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("tm_id").getUInteger(0));
+    } else if(messageName == "SYSTEM_TM_REQUEST_TC") {
+        mavlink_msg_system_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("tm_id").getUInteger(0));
         return true;
-    } else if(messageName == "SENSOR_TELEMETRY_REQUEST_TC") {
-        mavlink_msg_sensor_telemetry_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("sensor_id").getUInteger(0));
+    } else if(messageName == "SENSOR_TM_REQUEST_TC") {
+        mavlink_msg_sensor_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("sensor_id").getUInteger(0));
+        return true;
+    } else if(messageName == "SERVO_TM_REQUEST_TC") {
+        mavlink_msg_servo_tm_request_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0));
         return true;
     } else if(messageName == "SET_SERVO_ANGLE_TC") {
         mavlink_msg_set_servo_angle_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("servo_id").getUInteger(0), msg.getField("angle").getDouble(0.0));
@@ -58,6 +61,12 @@ bool MavlinkCommandAdapter::encodeCommand(const ModuleMessage& msg, mavlink_mess
     } else if(messageName == "RAW_EVENT_TC") {
         mavlink_msg_raw_event_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("topic_id").getUInteger(0), msg.getField("event_id").getUInteger(0));
         return true;
+    } else if(messageName == "SET_TARGET_COORDINATES_TC") {
+        mavlink_msg_set_target_coordinates_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("latitude").getUInteger(0), msg.getField("longitude").getUInteger(0));
+        return true;
+    } else if(messageName == "SET_ALGORITHM_TC") {
+        mavlink_msg_set_algorithm_tc_pack(MAV_SYS, MAV_CMP, &output, msg.getField("algorithm_number").getUInteger(0));
+        return true;
     }
     return false;
 }