From 63c1e3cd6e1bdacf7c9467819931465653ff551b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= <patrickelectric@gmail.com>
Date: Wed, 6 Mar 2024 11:18:44 -0300
Subject: [PATCH] GCS_MAVLink: GCS_Common: Add support to
 MAV_CMD_DO_SET_SYS_CMP_ID
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 29 ++++++++++++++++++++++++++++
 libraries/GCS_MAVLink/GCS_config.h   |  4 ++++
 3 files changed, 34 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index bd26264667b56..773522ee97a5c 100644
--- 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
index cf5074011055e..60485a0d21289 100644
--- 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_DO_SET_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->late_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
index 64877c10649dd..1d683ea9e4040 100644
--- 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_DO_SET_SYS_CMP_ID_ENABLED
+#define AP_MAVLINK_MAV_CMD_DO_SET_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