gimbal: correlate device messages to the right manager#2926
Open
julianoes wants to merge 3 commits into
Open
Conversation
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.
Collaborator
Author
|
Added a follow-up commit ( |
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Problem
GimbalImplkeys its discovery entries by the gimbal manager's component ID (whereGIMBAL_MANAGER_INFORMATIONarrives). But the two device-level messages —GIMBAL_DEVICE_INFORMATIONandGIMBAL_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_id7–255, i.e. the device has its own component ID). In that topology: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.new_item.gimbal_manager_compidwas set to the device's sender compid, so a laterGIMBAL_MANAGER_STATUS(from the manager compid) no longer matched and control status stopped updating.Fix
Correlate device messages by the manager-advertised
gimbal_device_idrather than the sender compid:gimbal_device_message_matches()helper encodes the protocol rule —gimbal_device_id1–6 means the device shares the manager's component;0means a separate device component identified by its own compid (which the manager advertises as itsgimbal_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 keyedfind) and, on promotion, takes the manager compid from the entry's key.Tests
Adds
gimbal_device_matching_test.cppcovering 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 (mirroringmission_import), rather than requiring aGimbalImpl/Systemin the test.A companion fix in the
gimbal_managerexample (using a 1–6 device id inGIMBAL_DEVICE_ATTITUDE_STATUSinstead of0) rides on the gimbal-manager branch.