diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/ArduSub/GCS_MAVLink_Sub.cpp index e08ff03e531e9..ea0cfdbef3a9e 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 04815e0fee063..e5f0ddd53da39 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 diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index aad7d64c10868..b95de7bb49c18 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)); diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 9b5dceacbef31..8ad1f5aa0c0c2 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 0d4ef1ca2b0b0..dd40d4ccf5d1a 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