From 588829161b4c50f3acebbd35e43a1827a186e829 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 14 Dec 2018 10:12:34 +1100 Subject: [PATCH] Rover: move accept_packet to GCS_MAVLink base class --- APMrover2/GCS_Mavlink.cpp | 18 ++++-------------- APMrover2/GCS_Mavlink.h | 2 +- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 39493ce317..75dbf0cc88 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -291,6 +291,10 @@ uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const { return rover.g.sysid_my_gcs; } +bool GCS_MAVLINK_Rover::sysid_enforce() const +{ + return rover.g2.sysid_enforce; +} uint32_t GCS_MAVLINK_Rover::telem_delay() const { @@ -1190,20 +1194,6 @@ void Rover::mavlink_delay_cb() DataFlash.EnableWrites(true); } -/* - 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); -} - AP_AdvancedFailsafe *GCS_MAVLINK_Rover::get_advanced_failsafe() const { #if ADVANCED_FAILSAFE == ENABLED diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index cb8411d66e..093eeb6ce0 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -12,13 +12,13 @@ public: protected: uint32_t telem_delay() const override; - bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) override; AP_Rally *get_rally() const override; AP_AdvancedFailsafe *get_advanced_failsafe() const override; AP_VisualOdom *get_visual_odom() const override; uint8_t sysid_my_gcs() const override; + bool sysid_enforce() const override; bool set_mode(uint8_t mode) override;