From 31a5cfe01352c1f0f2a130f88e89313fcb99a608 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 14 Jul 2026 21:09:47 +1200 Subject: [PATCH 1/3] gimbal: correlate device messages to the right manager Discovery entries are keyed by the gimbal manager's component ID (where GIMBAL_MANAGER_INFORMATION arrives). GIMBAL_DEVICE_INFORMATION and GIMBAL_DEVICE_ATTITUDE_STATUS, however, were looked up with _discovery.find(message.compid) on the device's sender compid. For a smart gimbal where the manager and device share a component this happens to work, but for a manager that advertises a separately-addressed gimbal device (gimbal_device_id 7-255 with its own component ID) the device messages arrive from a different compid, the lookup misses, and the gimbal is never announced. Even when promotion did happen, the item's gimbal_manager_compid was set to the device's compid, so a later GIMBAL_MANAGER_STATUS from the manager no longer matched and control status stopped updating. Correlate device messages by the advertised gimbal_device_id instead of the sender compid, via a shared gimbal_device_message_matches() helper used by both the announced-list matchers and discovery lookup, and take the manager compid from the discovery entry's key when promoting. Add unit tests covering the same-component (1-6) and separate-component (compid + id 0) topologies and cross-match rejection. --- cpp/src/mavsdk/plugins/gimbal/CMakeLists.txt | 6 ++ .../plugins/gimbal/gimbal_device_matching.cpp | 21 +++++ .../plugins/gimbal/gimbal_device_matching.hpp | 27 +++++++ .../gimbal/gimbal_device_matching_test.cpp | 44 +++++++++++ cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp | 79 +++++++++++-------- cpp/src/mavsdk/plugins/gimbal/gimbal_impl.hpp | 15 +++- 6 files changed, 155 insertions(+), 37 deletions(-) create mode 100644 cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.cpp create mode 100644 cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp create mode 100644 cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching_test.cpp diff --git a/cpp/src/mavsdk/plugins/gimbal/CMakeLists.txt b/cpp/src/mavsdk/plugins/gimbal/CMakeLists.txt index 1f49cadd49..43a9b4cc20 100644 --- a/cpp/src/mavsdk/plugins/gimbal/CMakeLists.txt +++ b/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) diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.cpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.cpp new file mode 100644 index 0000000000..1e18e40e51 --- /dev/null +++ b/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 diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp new file mode 100644 index 0000000000..21b362cc5e --- /dev/null +++ b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include + +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. +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 diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching_test.cpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching_test.cpp new file mode 100644 index 0000000000..f3dc7143b7 --- /dev/null +++ b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching_test.cpp @@ -0,0 +1,44 @@ +#include "gimbal_device_matching.hpp" +#include + +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)); +} diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index 7c18f5ee9a..106e1d355c 100644 --- a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/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" @@ -122,6 +123,18 @@ void GimbalImpl::try_request_gimbal_device_information( } } +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 lock(_mutex); @@ -265,10 +278,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,9 +293,10 @@ 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"); } @@ -289,11 +304,12 @@ void GimbalImpl::process_gimbal_device_information(const mavlink_message_t& mess } // 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; @@ -338,17 +354,19 @@ void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t& std::lock_guard 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,28 +375,21 @@ 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; + // 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)); + 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)); - 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); - }); + LogWarn("Continuing without GIMBAL_DEVICE_INFORMATION for compid {}", manager_compid); + _gimbal_list_subscriptions.queue(gimbal_list_with_lock(), [this](const auto& func) { + _system_impl->call_user_callback(func); + }); - maybe_gimbal = std::prev(_gimbals.end()); - } + maybe_gimbal = std::prev(_gimbals.end()); } if (maybe_gimbal == _gimbals.end()) { diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.hpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.hpp index cdc17a0cbb..4b6d699c9f 100644 --- a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.hpp +++ b/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; + 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 _attitude_subscriptions{_system_impl->io_context()}; std::vector _gimbals; - std::unordered_map _discovery; + DiscoveryMap _discovery; float _vehicle_yaw_rad{NAN}; bool _debugging{false}; From c95c93c2cadd7ef78413018e8b11cb6ab5306997 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 14 Jul 2026 21:17:28 +1200 Subject: [PATCH 2/3] gimbal: quiet discovery logging, add debug traces Two discovery warnings could fire at the attitude-status rate (e.g. 10 Hz): the invalid-gimbal_device_id warning and the unmatched-attitude warning. Move both behind MAVSDK_GIMBAL_DEBUGGING so they don't spam the log for a gimbal that streams an unexpected id or that isn't tracked. Turn the one-shot "Continuing without GIMBAL_DEVICE_INFORMATION" warning into a debug trace and add matching traces when a gimbal is announced (from GIMBAL_DEVICE_INFORMATION or via attitude-status fallback) and when GIMBAL_DEVICE_INFORMATION is requested, so the discovery handshake is easy to follow with debugging on. --- cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp | 44 ++++++++++++++++--- 1 file changed, 37 insertions(+), 7 deletions(-) diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index 106e1d355c..e5ad0c56cd 100644 --- a/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/cpp/src/mavsdk/plugins/gimbal/gimbal_impl.cpp @@ -119,6 +119,12 @@ 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); } } @@ -316,6 +322,14 @@ void GimbalImpl::process_gimbal_device_information(const mavlink_message_t& mess 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); }); @@ -344,10 +358,14 @@ 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; } @@ -384,7 +402,13 @@ void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t& // vendor/model/custom remain empty — no device info available. _gimbals.emplace_back(std::move(new_item)); - LogWarn("Continuing without GIMBAL_DEVICE_INFORMATION for compid {}", manager_compid); + 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); }); @@ -393,8 +417,14 @@ void GimbalImpl::process_gimbal_device_attitude_status(const mavlink_message_t& } 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; } From b92e5cd1c2a2ddc57941f9c990e04ffd841ec8fe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 14 Jul 2026 21:52:07 +1200 Subject: [PATCH 3/3] gimbal: export gimbal_device_message_matches for unit test libmavsdk builds with hidden visibility, so the free function was not visible to unit_tests_runner and linking failed with an undefined reference. Annotate it with MAVSDK_TEST_EXPORT, the same mechanism mission_import uses for its unit-tested helpers. --- cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp index 21b362cc5e..395d826a84 100644 --- a/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp +++ b/cpp/src/mavsdk/plugins/gimbal/gimbal_device_matching.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include "mavsdk_export.h" namespace mavsdk { @@ -18,7 +19,7 @@ namespace mavsdk { // 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. -bool gimbal_device_message_matches( +MAVSDK_TEST_EXPORT bool gimbal_device_message_matches( uint8_t manager_compid, uint8_t advertised_gimbal_device_id, uint8_t device_compid,