diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 2f9628407d..c673d66df3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -225,6 +225,10 @@ public: protected: + // overridable method to check for packet acceptance. Allows for + // enforcement of GCS sysid + virtual bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) { return true; } + bool waypoint_receiving; // currently receiving // the following two variables are only here because of Tracker uint16_t waypoint_request_i; // request index diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 26d01dc118..5ee3394541 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -985,7 +985,8 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, if (msg_snoop != NULL) { msg_snoop(&msg); } - if (routing.check_and_forward(chan, &msg)) { + if (routing.check_and_forward(chan, &msg) && + accept_packet(status, msg)) { handleMessage(&msg); } }