From 3a8e38ee120940d2643a4fa662eef74f41aad474 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 14 Aug 2016 07:52:03 +1000 Subject: [PATCH] Plane: added SYSID_ENFORCE parameter this allows safer sharing of telemetry links between aircraft --- ArduPlane/GCS_Mavlink.cpp | 14 ++++++++++++++ ArduPlane/GCS_Mavlink.h | 2 ++ ArduPlane/Parameters.cpp | 7 +++++++ ArduPlane/Parameters.h | 3 +++ 4 files changed, 26 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 273ed36d5c..a49626366c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -2432,3 +2432,17 @@ void Plane::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_Plane::accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) +{ + if (!plane.g2.sysid_enforce) { + return true; + } + if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + return true; + } + return (msg.sysid == plane.g.sysid_my_gcs); +} diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 5302cc54a7..1dbd4a4589 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -13,6 +13,8 @@ protected: uint32_t telem_delay() const override; + bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) override; + private: void handleMessage(mavlink_message_t * msg) override; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 21c476bf51..5f5dab871b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1382,6 +1382,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Group: SERVO_ // @Path: ../libraries/RC_Channel/SRV_Channel.cpp AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels), + + // @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", 4, ParametersG2, sysid_enforce, 0), AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 5bb6b5c2f0..0407c99786 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -588,6 +588,9 @@ public: // control over servo output ranges SRV_Channels servo_channels; + + // whether to enforce acceptance of packets only from sysid_my_gcs + AP_Int8 sysid_enforce; }; extern const AP_Param::Info var_info[];