GCS_MAVLink: GCS_Common: Add support to MAV_CMD_DO_SET_SYS_CMP_ID

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
ArduPilot · Mar 19, 2024 · 1798513 · 1798513
1 parent 2d24536
commit 1798513
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -597,6 +597,7 @@ class GCS_MAVLINK
 #endif
 
     MAV_RESULT handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
+    MAV_RESULT handle_do_set_sys_cmp_id(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
 
     // reset a message interval via mavlink:
     MAV_RESULT handle_command_set_message_interval(const mavlink_command_int_t &packet);

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -5173,6 +5173,32 @@ MAV_RESULT GCS_MAVLINK::handle_do_set_safety_switch_state(const mavlink_command_
     }
 }
 
+#if AP_MAVLINK_MAV_CMD_AWR_SYS_CMP_ID_ENABLED
+MAV_RESULT GCS_MAVLINK::handle_do_set_sys_cmp_id(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
+{
+    const uint8_t new_system_id = packet.param1;
+    const uint8_t new_component_id = packet.param2;
+    const uint8_t do_reboot = packet.param3;
+
+    if (new_system_id == 0 || hal.util->get_soft_armed()) {
+        return MAV_RESULT_DENIED;
+    }
+
+    if (!AP_Param::set_and_save_by_name_ifchanged("SYSID_THISMAV", new_system_id)) {
+        return MAV_RESULT_FAILED;
+    }
+
+    if (new_component_id != 0 && !AP_Param::set_and_save_by_name_ifchanged("COMPID_THISMAV", new_component_id)) {
+        return MAV_RESULT_FAILED;
+    }
+
+    if (do_reboot) {
+        hal.scheduler->reboot();
+    }
+
+    return MAV_RESULT_ACCEPTED;
+}
+#endif
 
 MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {
@@ -5334,6 +5360,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_DO_SET_SAFETY_SWITCH_STATE:
         return handle_do_set_safety_switch_state(packet, msg);
 
+    case MAV_CMD_DO_SET_SYS_CMP_ID:
+        return handle_do_set_sys_cmp_id(packet, msg);
+
 #if AP_MAVLINK_SERVO_RELAY_ENABLED
     case MAV_CMD_DO_SET_SERVO:
     case MAV_CMD_DO_REPEAT_SERVO:

diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h
@@ -42,6 +42,10 @@
 #define AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED 1
 #endif
 
+#ifndef AP_MAVLINK_MAV_CMD_AWR_SYS_CMP_ID_ENABLED
+#define AP_MAVLINK_MAV_CMD_AWR_SYS_CMP_ID_ENABLED 1
+#endif
+
 #ifndef HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
 #define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED ((AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) && BOARD_FLASH_SIZE > 1024)
 #endif