Rover: added SYSID_ENFORCE parameter

APMrover2/GCS_Mavlink.cpp

@@ -1567,3 +1567,17 @@ void Rover::gcs_retry_deferred(void)
     gcs_send_message(MSG_RETRY_DEFERRED);
     GCS_MAVLINK::service_statustext();
 }
+
+/*
+  return true if we will accept this packet. Used to implement SYSID_ENFORCE
+ */
+bool GCS_MAVLINK_Rover::accept_packet(const mavlink_status_t &status, mavlink_message_t &msg)
+{
+  if (!rover.g2.sysid_enforce) {
+    return true;
+  }
+  if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
+    return true;
+  }
+  return (msg.sysid == rover.g.sysid_my_gcs);
+}

APMrover2/GCS_Mavlink.h

@@ -15,6 +15,7 @@ public:
 protected:
 
     uint32_t telem_delay() const override;
+    bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) override;
 
 private:
 

APMrover2/Parameters.cpp

@@ -570,6 +570,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Stats/AP_Stats.cpp
     AP_SUBGROUPINFO(stats, "STAT", 1, ParametersG2, AP_Stats),
 
+    // @Group: SYSID_ENFORCE
+    // @DisplayName: GCS sysid enforcement
+    // @Description: This controls whether packets from other than the expected GCS system ID will be accepted
+    // @Values: 0:NotEnforced,1:Enforced
+    // @User: Advanced
+    AP_GROUPINFO("SYSID_ENFORCE", 2, ParametersG2, sysid_enforce, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -334,6 +334,9 @@ public:
     // vehicle statistics
     AP_Stats stats;
 
+    // whether to enforce acceptance of packets only from sysid_my_gcs
+    AP_Int8 sysid_enforce;
+
 };