From 326e0f224f8b61221649bf3ba1f1eb57c4a38820 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Mon, 21 Nov 2016 16:06:25 +0100
Subject: [PATCH] Rover: added SYSID_ENFORCE parameter

---
 APMrover2/GCS_Mavlink.cpp | 14 ++++++++++++++
 APMrover2/GCS_Mavlink.h   |  1 +
 APMrover2/Parameters.cpp  |  7 +++++++
 APMrover2/Parameters.h    |  3 +++
 4 files changed, 25 insertions(+)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 5fda512ecc..db7b839a66 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/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);
+}
\ No newline at end of file
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index a733c93758..2c0fc4542e 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/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:
 
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 712663c647..05857e4ddb 100644
--- a/APMrover2/Parameters.cpp
+++ b/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
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 3bc84b8861..a7eae85195 100644
--- a/APMrover2/Parameters.h
+++ b/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;
+
 };