From 3ef74d4897495ede9a72321cd68c026ff5394ae7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 13 Jul 2025 09:03:06 +1000
Subject: [PATCH 1/3] GCS_MAVLink: added MAV_GCS_SYSID_HI

this allows for multiple MAVLink source system IDs to be treated as
valid GCS nodes, allowing for multiple ground stations for GCS
failsafe handling
---
 libraries/GCS_MAVLink/GCS.cpp        | 25 ++++++++++++++++++++++---
 libraries/GCS_MAVLink/GCS.h          |  6 +++++-
 libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++-----
 3 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index 9b5dceacbef310..8ad1f5aa0c0c28 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -38,23 +38,31 @@ extern const AP_HAL::HAL& hal;
 const AP_Param::GroupInfo GCS::var_info[] {
     // @Param: _SYSID
     // @DisplayName: MAVLink system ID of this vehicle
-    // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
+    // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network.
     // @Range: 1 255
     // @User: Advanced
     AP_GROUPINFO("_SYSID",    1,     GCS,  sysid,  MAV_SYSID_DEFAULT),
 
     // @Param: _GCS_SYSID
     // @DisplayName: My ground station number
-    // @Description: This controls whether packets from other than the expected GCS system ID will be accepted
+    // @Description: This sets what MAVLink source system IDs are accepted for GCS failsafe handling, RC overrides and manual control. When MAV_GCS_SYSID_HI is less than MAV_GCS_SYSID then only this value is considered to be a GCS. When MAV_GCS_SYSID_HI is greater than or equal to MAV_GCS_SYSID then the range of values between MAV_GCS_SYSID and MAV_GCS_SYSID_HI (inclusive) are all treated as valid GCS MAVLink system IDs
     // @Range: 1 255
     // @Increment: 1
     // @User: Advanced
     AP_GROUPINFO("_GCS_SYSID",    2,      GCS, mav_gcs_sysid, 255),
 
+    // @Param: _GCS_SYSID_HI
+    // @DisplayName: ground station system ID, maximum
+    // @Description: Upper limit of MAVLink source system IDs considered to be from the GCS. When this is less than MAV_GCS_SYSID then only MAV_GCS_SYSID is used as GCS ID. When this is greater than or equal to MAV_GCS_SYSID then the range of values from MAV_GCS_SYSID to MAV_GCS_SYSID_HI (inclusive) is treated as a GCS ID.
+    // @Range: 0 255
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("_GCS_SYSID_HI",    5,   GCS, mav_gcs_sysid_high, 0),
+    
     // @Param: _OPTIONS
     // @DisplayName: MAVLink Options
     // @Description: Alters various behaviour of the MAVLink interface
-    // @Bitmask: 0:Accept MAVLink only from SYSID_GCS
+    // @Bitmask: 0:Accept MAVLink only from system IDs given by MAV_SYSID_GCS and MAV_SYSID_GCS_HI
     // @User: Advanced
     AP_GROUPINFO("_OPTIONS",    3,      GCS, mav_options, 0),
 
@@ -545,4 +553,15 @@ MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet)
 }
 #endif // AP_SCRIPTING_ENABLED
 
+/*
+  return true if a MAVLink system ID is a GCS for this vehicle
+*/
+bool GCS::sysid_is_gcs(uint8_t _sysid) const
+{
+    if (mav_gcs_sysid_high <= mav_gcs_sysid) {
+        return mav_gcs_sysid == _sysid;
+    }
+    return _sysid >= mav_gcs_sysid && _sysid <= mav_gcs_sysid_high;
+}
+
 #endif  // HAL_GCS_ENABLED
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 0d4ef1ca2b0b00..dd40d4ccf5d1ad 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -1210,7 +1210,10 @@ class GCS
         return _statustext_queue;
     }
 
-    uint8_t sysid_gcs() const { return uint8_t(mav_gcs_sysid); }
+    /*
+      return true if a MAVLink system ID is a GCS
+     */
+    bool sysid_is_gcs(uint8_t sysid) const;
 
     // last time traffic was seen from my designated GCS.  traffic
     // includes heartbeats and some manual control messages.
@@ -1364,6 +1367,7 @@ class GCS
     // parameters
     AP_Int16                 sysid;
     AP_Int16                 mav_gcs_sysid;
+    AP_Int16                 mav_gcs_sysid_high;
     AP_Enum16<Option>        mav_options;
     AP_Int8                  mav_telem_delay;
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index bc3b318d0aa8b4..f889e90fb3038c 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3798,7 +3798,7 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const
     char text[text_len] = { 'G','C','S',':'};
     uint8_t offset = strlen(text);
 
-    if (msg.sysid != gcs().sysid_gcs()) {
+    if (!gcs().sysid_is_gcs(msg.sysid)) {
         offset = hal.util->snprintf(text,
                                     max_prefix_len,
                                     "SRC=%u/%u:",
@@ -4104,7 +4104,7 @@ void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)
 // control of switch position and RC PWM values.
 void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
 {
-    if(msg.sysid != gcs().sysid_gcs()) {
+    if (!gcs().sysid_is_gcs(msg.sysid)) {
         return; // Only accept control from our gcs
     }
 
@@ -4267,7 +4267,7 @@ void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg)
 {
     // if the heartbeat is from our GCS then we don't failsafe for
     // now...
-    if (msg.sysid == gcs().sysid_gcs()) {
+    if (gcs().sysid_is_gcs(msg.sysid)) {
         sysid_mygcs_seen(AP_HAL::millis());
     }
 }
@@ -7075,7 +7075,7 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status,
         return true;
     }
 
-    if (msg.sysid == gcs().sysid_gcs()) {
+    if (gcs().sysid_is_gcs(msg.sysid)) {
         return true;
     }
 
@@ -7327,7 +7327,7 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
 
 void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg)
 {
-    if (msg.sysid != gcs().sysid_gcs()) {
+    if (!gcs().sysid_is_gcs(msg.sysid)) {
         return; // only accept control from our gcs
     }
 

From a436fd7f295caec54d2c09345c1b0d90b4a60d24 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 13 Jul 2025 09:03:19 +1000
Subject: [PATCH 2/3] Rover: update comment

---
 Rover/Rover.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp
index aad7d64c108688..b95de7bb49c18a 100644
--- a/Rover/Rover.cpp
+++ b/Rover/Rover.cpp
@@ -358,7 +358,7 @@ void Rover::gcs_failsafe_check(void)
     }
 
     // calc time since last gcs update
-    // note: this only looks at the heartbeat from the device id set by gcs().sysid_gcs()
+    // note: this only looks at the heartbeat from the device ids approved by gcs().sysid_is_gcs()
     const uint32_t last_gcs_update_ms = millis() - gcs_last_seen_ms;
     const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
 

From 3b1525877623735a169d55f5b48c90ac2fcb1b7c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 13 Jul 2025 09:03:48 +1000
Subject: [PATCH 3/3] Sub: update for sysid_is_gcs()

---
 ArduSub/GCS_MAVLink_Sub.cpp | 4 ++--
 ArduSub/failsafe.cpp        | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/ArduSub/GCS_MAVLink_Sub.cpp
index e08ff03e531e90..ea0cfdbef3a9ee 100644
--- a/ArduSub/GCS_MAVLink_Sub.cpp
+++ b/ArduSub/GCS_MAVLink_Sub.cpp
@@ -457,7 +457,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
     switch (msg.msgid) {
 
     case MAVLINK_MSG_ID_MANUAL_CONTROL: {     // MAV ID: 69
-        if (msg.sysid != gcs().sysid_gcs()) {
+        if (!gcs().sysid_is_gcs(msg.sysid)) {
             break;    // Only accept control from our gcs
         }
         mavlink_manual_control_t packet;
@@ -493,7 +493,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
     }
 
     case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {     // MAV ID: 70
-        if (msg.sysid != gcs().sysid_gcs()) {
+        if (!gcs().sysid_is_gcs(msg.sysid)) {
             break;    // Only accept control from our gcs
         }
 
diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
index 04815e0fee063f..e5f0ddd53da394 100644
--- a/ArduSub/failsafe.cpp
+++ b/ArduSub/failsafe.cpp
@@ -337,7 +337,7 @@ void Sub::failsafe_gcs_check()
     // Send a warning every 30 seconds
     if (tnow - failsafe.last_gcs_warn_ms > 30000) {
         failsafe.last_gcs_warn_ms = tnow;
-        gcs().send_text(MAV_SEVERITY_WARNING, "MYGCS: %u, heartbeat lost", unsigned(gcs().sysid_gcs()));
+        gcs().send_text(MAV_SEVERITY_WARNING, "MYGCS: heartbeat lost");
     }
 
     // do nothing if we have already triggered the failsafe action, or if the motors are disarmed