From c034f8a099ba90ac502dd31fbe4aed89b8d46311 Mon Sep 17 00:00:00 2001
From: olliw42 <waldmanns@gmx.de>
Date: Wed, 7 Sep 2022 17:18:53 +0900
Subject: [PATCH] GCS_MAVLink: add find_by_mavtype_and_compid

---
 libraries/GCS_MAVLink/GCS.h               |  6 ++++++
 libraries/GCS_MAVLink/MAVLink_routing.cpp | 16 ++++++++++++++++
 libraries/GCS_MAVLink/MAVLink_routing.h   |  6 ++++++
 3 files changed, 28 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 21dbca0de7..ddbb9526f9 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -383,6 +383,12 @@ public:
      */
     static bool find_by_mavtype(uint8_t mav_type, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel) { return routing.find_by_mavtype(mav_type, sysid, compid, channel); }
 
+    /*
+      search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
+      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); }
+
     // update signing timestamp on GPS lock
     static void update_signing_timestamp(uint64_t timestamp_usec);
 
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
index 934d55d268..060ba7c4ea 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.cpp
+++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp
@@ -262,6 +262,22 @@ bool MAVLink_routing::find_by_mavtype(uint8_t mavtype, uint8_t &sysid, uint8_t &
     return false;
 }
 
+/*
+  search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
+  returns true if a match is found
+ */
+bool MAVLink_routing::find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) const
+{
+    for (uint8_t i=0; i<num_routes; i++) {
+        if ((routes[i].mavtype == mavtype) && (routes[i].compid == compid)) {
+            sysid = routes[i].sysid;
+            channel = routes[i].channel;
+            return true;
+        }
+    }
+    return false;
+}
+
 /*
   see if the message is for a new route and learn it
 */
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h
index 925ae27010..3c51b77ac8 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.h
+++ b/libraries/GCS_MAVLink/MAVLink_routing.h
@@ -43,6 +43,12 @@ public:
      */
     bool find_by_mavtype(uint8_t mavtype, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel);
 
+    /*
+      search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
+      returns true if a match is found
+     */
+    bool find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) const;
+
 private:
     // a simple linear routing table. We don't expect to have a lot of
     // routes, so a scalable structure isn't worthwhile yet.