gimbal: correlate device messages to the right manager

cpp/src/mavsdk/plugins/gimbal/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(mavsdk
     PRIVATE
     gimbal.cpp
     gimbal_impl.cpp
+    gimbal_device_matching.cpp
 )
 
 target_include_directories(mavsdk PUBLIC
@@ -13,3 +14,8 @@ install(FILES
     include/plugins/gimbal/gimbal.hpp
     DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/gimbal
 )
+
+list(APPEND UNIT_TEST_SOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_device_matching_test.cpp
+)
+set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE)

cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.cpp

@@ -0,0 +1,21 @@
+#include "gimbal_device_matching.hpp"
+
+namespace mavsdk {
+
+bool gimbal_device_message_matches(
+    uint8_t manager_compid,
+    uint8_t advertised_gimbal_device_id,
+    uint8_t device_compid,
+    uint8_t device_msg_gimbal_device_id)
+{
+    if (device_msg_gimbal_device_id == 0) {
+        // Separate gimbal device component: identified by its own compid, which the
+        // manager advertised as its gimbal_device_id.
+        return advertised_gimbal_device_id == device_compid;
+    }
+    // Non-MAVLink gimbal device (id 1-6): it shares the manager's component.
+    return manager_compid == device_compid &&
+           advertised_gimbal_device_id == device_msg_gimbal_device_id;
+}
+
+} // namespace mavsdk

cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <cstdint>
+#include "mavsdk_export.h"
+
+namespace mavsdk {
+
+// Correlate a GIMBAL_DEVICE_* message (GIMBAL_DEVICE_INFORMATION or
+// GIMBAL_DEVICE_ATTITUDE_STATUS) back to the gimbal a manager advertised.
+//
+// A gimbal manager announces which device it is responsible for via
+// GIMBAL_MANAGER_INFORMATION.gimbal_device_id (here: advertised_gimbal_device_id):
+//   - 1-6:   a non-MAVLink gimbal device that shares the manager's component, so the
+//            device's messages come from the manager's component ID and carry the same
+//            1-6 id.
+//   - 7-255: a gimbal device that is its own MAVLink component; its messages come from
+//            that component ID and carry a gimbal_device_id of 0 (device_msg_gimbal_device_id).
+//
+// Returns whether the gimbal identified by (manager_compid, advertised_gimbal_device_id)
+// is the one that sent a device message from device_compid carrying
+// device_msg_gimbal_device_id.
+MAVSDK_TEST_EXPORT bool gimbal_device_message_matches(
+    uint8_t manager_compid,
+    uint8_t advertised_gimbal_device_id,
+    uint8_t device_compid,
+    uint8_t device_msg_gimbal_device_id);
+
+} // namespace mavsdk

cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching_test.cpp

@@ -0,0 +1,44 @@
+#include "gimbal_device_matching.hpp"
+#include <gtest/gtest.h>
+
+using namespace mavsdk;
+
+// Component IDs from MAV_COMPONENT for readability.
+static constexpr uint8_t comp_autopilot = 1;
+static constexpr uint8_t comp_gimbal = 154;
+static constexpr uint8_t comp_gimbal2 = 171;
+
+// Same component acts as manager and device (e.g. a smart gimbal). The manager advertises a
+// non-MAVLink device id in 1-6, and the device messages carry that same id.
+TEST(GimbalDeviceMatching, SameComponentMatchesOnDeviceId)
+{
+    // Manager compid 154 advertises device id 1; attitude status comes from 154 with id 1.
+    EXPECT_TRUE(gimbal_device_message_matches(comp_gimbal, 1, comp_gimbal, 1));
+
+    // Wrong device id from the same component does not match.
+    EXPECT_FALSE(gimbal_device_message_matches(comp_gimbal, 1, comp_gimbal, 2));
+
+    // Right device id but from a different component does not match.
+    EXPECT_FALSE(gimbal_device_message_matches(comp_gimbal, 1, comp_gimbal2, 1));
+}
+
+// Separate device component: the manager (e.g. the autopilot) advertises the device's own
+// component ID, and the device streams messages with gimbal_device_id 0.
+TEST(GimbalDeviceMatching, SeparateComponentMatchesOnCompid)
+{
+    // Autopilot manager advertises device compid 154; device streams from 154 with id 0.
+    EXPECT_TRUE(gimbal_device_message_matches(comp_autopilot, comp_gimbal, comp_gimbal, 0));
+
+    // A message from a different device component (id 0) must not be attributed to this gimbal.
+    EXPECT_FALSE(gimbal_device_message_matches(comp_autopilot, comp_gimbal, comp_gimbal2, 0));
+}
+
+// Two managers each with their own gimbal device must stay distinct.
+TEST(GimbalDeviceMatching, DistinctGimbalsDoNotCrossMatch)
+{
+    // Same-component gimbal 1 vs a device message that belongs to a separate-component gimbal.
+    EXPECT_FALSE(gimbal_device_message_matches(comp_autopilot, comp_gimbal, comp_gimbal, 1));
+
+    // Separate-component gimbal (advertising compid 171) vs a same-component device message.
+    EXPECT_FALSE(gimbal_device_message_matches(comp_autopilot, comp_gimbal2, comp_gimbal2, 1));
+}

cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp

@@ -1,4 +1,5 @@
 #include "gimbal_impl.hpp"
+#include "gimbal_device_matching.hpp"
 #include "callback_list.tpp"
 #include "mavsdk_export.h"
 #include "math_utils.hpp"
@@ -118,10 +119,28 @@ void GimbalImpl::try_request_gimbal_device_information(
             manager_compid :
             discovery.gimbal_device_id;
     if (target_component_id != 0) {
+        if (_debugging) {
+            LogDebug(
+                "Requesting GIMBAL_DEVICE_INFORMATION for gimbal_device_id {} from compid {}",
+                discovery.gimbal_device_id,
+                target_component_id);
+        }
         request_gimbal_device_information(target_component_id);
     }
 }
 
+GimbalImpl::DiscoveryMap::iterator GimbalImpl::find_pending_discovery_for_device(
+    uint8_t device_compid, uint8_t device_msg_gimbal_device_id)
+{
+    return std::find_if(_discovery.begin(), _discovery.end(), [&](const auto& entry) {
+        return entry.second.has_manager_info && gimbal_device_message_matches(
+                                                    entry.first,
+                                                    entry.second.gimbal_device_id,
+                                                    device_compid,
+                                                    device_msg_gimbal_device_id);
+    });
+}
+
 void GimbalImpl::process_heartbeat(const mavlink_message_t& message)
 {
     std::lock_guard<std::mutex> lock(_mutex);
@@ -265,10 +284,11 @@ void GimbalImpl::process_gimbal_device_information(const mavlink_message_t& mess
 
     // Check if already announced — just update device info in that case.
     auto it = std::find_if(_gimbals.begin(), _gimbals.end(), [&](const GimbalItem& item) {
-        if (gimbal_device_information.gimbal_device_id == 0) {
-            return item.gimbal_device_id == message.compid;
-        }
-        return item.gimbal_manager_compid == message.compid;
+        return gimbal_device_message_matches(
+            item.gimbal_manager_compid,
+            item.gimbal_device_id,
+            message.compid,
+            gimbal_device_information.gimbal_device_id);
     });
 
     if (it != _gimbals.end()) {
@@ -279,27 +299,37 @@ void GimbalImpl::process_gimbal_device_information(const mavlink_message_t& mess
         return;
     }
 
-    // Look for a pending discovery entry for this compid.
-    auto disc_it = _discovery.find(message.compid);
-    if (disc_it == _discovery.end() || !disc_it->second.has_manager_info) {
+    // Look for a pending discovery entry that this gimbal device belongs to.
+    auto disc_it = find_pending_discovery_for_device(
+        message.compid, gimbal_device_information.gimbal_device_id);
+    if (disc_it == _discovery.end()) {
         if (_debugging) {
             LogDebug("Didn't find pending gimbal for gimbal device");
         }
         return;
     }
 
     // Promote from pending to announced.
+    const uint8_t manager_compid = disc_it->first;
     auto& discovery = disc_it->second;
     discovery.device_info_requests_left = 0;
 
     GimbalItem new_item{};
-    new_item.gimbal_manager_compid = message.compid;
+    new_item.gimbal_manager_compid = manager_compid;
     new_item.gimbal_device_id = discovery.gimbal_device_id;
     new_item.vendor_name = gimbal_device_information.vendor_name;
     new_item.model_name = gimbal_device_information.model_name;
     new_item.custom_name = gimbal_device_information.custom_name;
     _gimbals.emplace_back(std::move(new_item));
 
+    if (_debugging) {
+        LogDebug(
+            "Announcing gimbal (manager compid {}, gimbal_device_id {}) from "
+            "GIMBAL_DEVICE_INFORMATION: {}",
+            manager_compid,
+            discovery.gimbal_device_id,
+            gimbal_device_information.model_name);
+    }
     _gimbal_list_subscriptions.queue(gimbal_list_with_lock(), [this](const auto& func) {
         _system_impl->call_user_callback(func);
     });
@@ -328,27 +358,33 @@ void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t&
     }
 
     if (attitude_status.gimbal_device_id > 6) {
-        LogWarn(
-            "Ignoring gimbal device attitude status with invalid gimbal_device_id {} from ({}/)",
-            attitude_status.gimbal_device_id,
-            message.sysid);
+        // This streams at the attitude rate (e.g. 10 Hz), so keep it behind debugging.
+        if (_debugging) {
+            LogDebug(
+                "Ignoring gimbal device attitude status with invalid gimbal_device_id {} from {}/{}",
+                attitude_status.gimbal_device_id,
+                message.sysid,
+                message.compid);
+        }
         return;
     }
 
     std::lock_guard<std::mutex> lock(_mutex);
 
     auto maybe_gimbal = std::find_if(_gimbals.begin(), _gimbals.end(), [&](const GimbalItem& item) {
-        if (attitude_status.gimbal_device_id == 0) {
-            return item.gimbal_device_id == message.compid;
-        }
-        return item.gimbal_manager_compid == message.compid &&
-               item.gimbal_device_id == attitude_status.gimbal_device_id;
+        return gimbal_device_message_matches(
+            item.gimbal_manager_compid,
+            item.gimbal_device_id,
+            message.compid,
+            attitude_status.gimbal_device_id);
     });
 
     if (maybe_gimbal == _gimbals.end()) {
         // Not yet announced. Check if there is a matching pending discovery entry and promote it.
-        auto disc_it = _discovery.find(message.compid);
-        if (disc_it != _discovery.end() && disc_it->second.has_manager_info) {
+        auto disc_it =
+            find_pending_discovery_for_device(message.compid, attitude_status.gimbal_device_id);
+        if (disc_it != _discovery.end()) {
+            const uint8_t manager_compid = disc_it->first;
             auto& discovery = disc_it->second;
 
             if (discovery.device_info_requests_left > 0) {
@@ -357,33 +393,38 @@ void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t&
                 return;
             }
 
-            const bool device_id_matches =
-                attitude_status.gimbal_device_id == 0 ||
-                discovery.gimbal_device_id == attitude_status.gimbal_device_id;
-
-            if (device_id_matches) {
-                // Promote: we have confirmed the gimbal via attitude status.
-                discovery.device_info_requests_left = 0;
-
-                GimbalItem new_item{};
-                new_item.gimbal_manager_compid = message.compid;
-                new_item.gimbal_device_id = discovery.gimbal_device_id;
-                // vendor/model/custom remain empty — no device info available.
-                _gimbals.emplace_back(std::move(new_item));
+            // Promote: we have confirmed the gimbal via attitude status.
+            discovery.device_info_requests_left = 0;
 
-                LogWarn(
-                    "Continuing without GIMBAL_DEVICE_INFORMATION for compid {}", message.compid);
-                _gimbal_list_subscriptions.queue(gimbal_list_with_lock(), [this](const auto& func) {
-                    _system_impl->call_user_callback(func);
-                });
+            GimbalItem new_item{};
+            new_item.gimbal_manager_compid = manager_compid;
+            new_item.gimbal_device_id = discovery.gimbal_device_id;
+            // vendor/model/custom remain empty — no device info available.
+            _gimbals.emplace_back(std::move(new_item));
 
-                maybe_gimbal = std::prev(_gimbals.end());
+            if (_debugging) {
+                LogDebug(
+                    "Announcing gimbal (manager compid {}, gimbal_device_id {}) via "
+                    "GIMBAL_DEVICE_ATTITUDE_STATUS without GIMBAL_DEVICE_INFORMATION.",
+                    manager_compid,
+                    discovery.gimbal_device_id);
             }
+            _gimbal_list_subscriptions.queue(gimbal_list_with_lock(), [this](const auto& func) {
+                _system_impl->call_user_callback(func);
+            });
+
+            maybe_gimbal = std::prev(_gimbals.end());
         }
 
         if (maybe_gimbal == _gimbals.end()) {
-            if (!_gimbals.empty()) {
-                LogWarn("Received gimbal device attitude status for unknown gimbal.");
+            // Also at the attitude rate — only surface it when debugging.
+            if (_debugging && !_gimbals.empty()) {
+                LogDebug(
+                    "Received gimbal device attitude status from {}/{} (gimbal_device_id {}) "
+                    "with no matching gimbal.",
+                    message.sysid,
+                    message.compid,
+                    attitude_status.gimbal_device_id);
             }
             return;
         }

cpp/src/mavsdk/plugins/gimbal/gimbal_impl.hpp

@@ -118,6 +118,9 @@ class GimbalImpl : public PluginImplBase {
         unsigned device_info_requests_left{0};
     };
 
+    // Keyed by the gimbal manager's component ID (where GIMBAL_MANAGER_INFORMATION arrives).
+    using DiscoveryMap = std::unordered_map<uint8_t, GimbalDiscovery>;
+
     struct GimbalAddress {
         uint8_t gimbal_manager_compid{0};
         uint8_t gimbal_device_id{0};
@@ -127,8 +130,14 @@ class GimbalImpl : public PluginImplBase {
 
     void request_gimbal_manager_information(uint8_t target_component_id) const;
     void request_gimbal_device_information(uint8_t target_component_id) const;
-    void try_request_gimbal_device_information(
-        GimbalDiscovery& discovery, uint8_t manager_compid) const;
+    void
+    try_request_gimbal_device_information(GimbalDiscovery& discovery, uint8_t manager_compid) const;
+
+    // Find the pending discovery entry that owns a GIMBAL_DEVICE_* message. Returns
+    // _discovery.end() if none matches. The entry's key is the manager compid, which may differ
+    // from device_compid.
+    DiscoveryMap::iterator
+    find_pending_discovery_for_device(uint8_t device_compid, uint8_t device_msg_gimbal_device_id);
 
     void process_heartbeat(const mavlink_message_t& message);
     void process_gimbal_manager_information(const mavlink_message_t& message);
@@ -166,7 +175,7 @@ class GimbalImpl : public PluginImplBase {
     CallbackList<Gimbal::Attitude> _attitude_subscriptions{_system_impl->io_context()};
 
     std::vector<GimbalItem> _gimbals;
-    std::unordered_map<uint8_t, GimbalDiscovery> _discovery;
+    DiscoveryMap _discovery;
     float _vehicle_yaw_rad{NAN};
 
     bool _debugging{false};