Skip to content

gimbal: correlate device messages to the right manager#2926

Open
julianoes wants to merge 3 commits into
mainfrom
pr-gimbal-device-correlation
Open

gimbal: correlate device messages to the right manager#2926
julianoes wants to merge 3 commits into
mainfrom
pr-gimbal-device-correlation

Conversation

@julianoes

Copy link
Copy Markdown
Collaborator

Problem

GimbalImpl keys its discovery entries by the gimbal manager's component ID (where GIMBAL_MANAGER_INFORMATION arrives). But the two device-level messages — GIMBAL_DEVICE_INFORMATION and GIMBAL_DEVICE_ATTITUDE_STATUS — were looked up with _discovery.find(message.compid) on the device's sender compid.

For a smart gimbal where the manager and the device are the same component this happens to work (sender compid == manager compid). But the gimbal protocol v2 also allows a manager that is responsible for a separately-addressed gimbal device (gimbal_device_id 7–255, i.e. the device has its own component ID). In that topology:

  • The device's GIMBAL_DEVICE_* messages arrive from a different compid than the manager, so _discovery.find(message.compid) misses the entry and the gimbal is never announced.
  • Even where promotion did happen, new_item.gimbal_manager_compid was set to the device's sender compid, so a later GIMBAL_MANAGER_STATUS (from the manager compid) no longer matched and control status stopped updating.

Fix

Correlate device messages by the manager-advertised gimbal_device_id rather than the sender compid:

  • New gimbal_device_message_matches() helper encodes the protocol rule — gimbal_device_id 1–6 means the device shares the manager's component; 0 means a separate device component identified by its own compid (which the manager advertises as its gimbal_device_id). Used by both the announced-list matchers and the new discovery lookup.
  • find_pending_discovery_for_device() searches discovery entries (not a compid keyed find) and, on promotion, takes the manager compid from the entry's key.

Tests

Adds gimbal_device_matching_test.cpp covering the same-component (1–6), separate-component (compid + id 0), and cross-match-rejection cases.

Notes

Extracted as a standalone gimbal_device_matching.{hpp,cpp} unit so the correlation logic is directly unit-testable (mirroring mission_import), rather than requiring a GimbalImpl/System in the test.

A companion fix in the gimbal_manager example (using a 1–6 device id in GIMBAL_DEVICE_ATTITUDE_STATUS instead of 0) rides on the gimbal-manager branch.

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.
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.
@julianoes

Copy link
Copy Markdown
Collaborator Author

Added a follow-up commit (c95c93c2c): moved two discovery warnings that can fire at the attitude-status rate (invalid gimbal_device_id, and unmatched attitude status) behind MAVSDK_GIMBAL_DEBUGGING, turned the one-shot "Continuing without GIMBAL_DEVICE_INFORMATION" warning into a debug trace, and added debug traces for device-info requests and gimbal announcements so the discovery handshake is easy to follow with debugging on.

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.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant