diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 9d280e5e6a..683494590c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -444,6 +444,9 @@ public: returns true if a match is found */ static bool find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) { return routing.find_by_mavtype_and_compid(mav_type, compid, sysid, channel); } + // same as above, but returns a pointer to the GCS_MAVLINK object + // corresponding to the channel + static GCS_MAVLINK *find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid); // update signing timestamp on GPS lock static void update_signing_timestamp(uint64_t timestamp_usec); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 225272e939..e93b26f7ec 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -48,6 +48,14 @@ mavlink_system_t mavlink_system = {7,1}; // routing table MAVLink_routing GCS_MAVLINK::routing; +GCS_MAVLINK *GCS_MAVLINK::find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid) { + mavlink_channel_t channel; + if (!routing.find_by_mavtype_and_compid(mav_type, compid, sysid, channel)) { + return nullptr; + } + return gcs().chan(channel); +} + // set a channel as private. Private channels get sent heartbeats, but // don't get broadcast packets or forwarded packets void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)