Skip to content

mocap: wire up reset_counter, estimator_type, quality fields#2791

Merged
julianoes merged 1 commit intomavlink:mainfrom
alireza787b:mocap-wire-up-new-fields
Apr 1, 2026
Merged

mocap: wire up reset_counter, estimator_type, quality fields#2791
julianoes merged 1 commit intomavlink:mainfrom
alireza787b:mocap-wire-up-new-fields

Conversation

@alireza787b
Copy link
Copy Markdown
Contributor

@alireza787b alireza787b commented Mar 10, 2026

Problem

The Mocap plugin hardcodes several fields when packing MAVLink messages,
ignoring values that PX4's EKF2 can use for better sensor fusion:

send_vision_position_estimate:

0); // FIXME: reset_counter not set   ← line 119

send_vision_speed_estimate:

0); // FIXME: reset_counter not set   ← line 170

send_odometry:

0,                        // reset_counter hardcoded
MAV_ESTIMATOR_TYPE_MOCAP, // estimator_type hardcoded (wrong for VIO users)
0);                       // quality hardcoded

Additionally, static_cast<uint8_t>(odometry.frame_id) sends the proto enum
value (0) instead of the MAVLink value (14 = MAV_FRAME_MOCAP_NED), because
the proto enum uses sequential values rather than MAVLink values.

Changes

  1. Wire up reset_counter in send_vision_position_estimate — resolves
    the FIXME comment

  2. Wire up reset_counter in send_vision_speed_estimate — resolves
    the FIXME comment

  3. Wire up reset_counter, estimator_type, quality in send_odometry
    — replaces all three hardcoded values with the proto fields

  4. Fix frame_id mapping — add switch to map proto MavFrame enum values
    to correct MAVLink MAV_FRAME constants (14 for MOCAP_NED, 20 for LOCAL_FRD)

  5. Backward compatibility — when estimator_type is UNKNOWN (proto3 default),
    fall back to MAV_ESTIMATOR_TYPE_MOCAP to preserve existing behavior

Dependencies

Related PRs

Testing

After proto submodule update:

cmake -Bbuild/default -S. -DBUILD_MAVSDK_SERVER=ON -DWERROR=ON
cmake --build build/default -j$(nproc)

Verified: all changes maintain backward compatibility — existing users who
don't set the new fields get identical MAVLink output (reset_counter=0,
estimator_type=MOCAP, quality=0, frame_id=MOCAP_NED).

JonasVautherin
JonasVautherin previously approved these changes Mar 11, 2026
@alireza787b
Copy link
Copy Markdown
Contributor Author

Hi — just a note that the CI failures here are expected: this PR references reset_counter, estimator_type, and quality_percent fields that were added in MAVSDK-Proto PR #395, which hasn't been merged yet.

Once #395 is merged and the proto submodule pointer is updated, the build should pass.

Dependency chain:

  1. MAVSDK-Proto fly_qgc_mission: link to json11 #395 (merge) → update proto submodule in MAVSDK → this PR's CI passes
  2. Then MAVSDK-Python Trying out GitLab CI #820 can use the new fields

Happy to rebase or address anything else in the meantime.

Copy link
Copy Markdown
Collaborator

@julianoes julianoes left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried to merge this and it didn't compile, see comment mavlink/MAVSDK-Proto#395 (comment).

@julianoes julianoes marked this pull request as draft March 29, 2026 16:56
@alireza787b
Copy link
Copy Markdown
Contributor Author

Hey Juliano, you are right. This branch is stale in its current form and the old dependency assumption was not enough. I re-checked the proto/C++/generation path end-to-end and I would rather replace this with clean current-main PRs than keep patching stale history here. I will prepare a small fresh MAVSDK-Proto PR first, then a regenerated MAVSDK follow-up that keeps the frame mapping in C++. Very happy to align with any suggestion on that split if you prefer a different sequencing.

@julianoes
Copy link
Copy Markdown
Collaborator

I've merged proto, you can now fix this up.

@julianoes julianoes force-pushed the mocap-wire-up-new-fields branch from 7d65884 to f2b9178 Compare April 1, 2026 01:16
@julianoes julianoes marked this pull request as ready for review April 1, 2026 01:16
@julianoes
Copy link
Copy Markdown
Collaborator

@alireza787b I've rebased this and fixed it up. Hopefully that works for you like that. I want to get this into the next v3 release, so I went ahead.

…oto fields

Replace hardcoded values with the corresponding proto fields added
in mavlink/MAVSDK-Proto#395:

- send_vision_position_estimate: use reset_counter field instead of
  hardcoded 0

- send_vision_speed_estimate: use reset_counter field instead of
  hardcoded 0

- send_odometry: use reset_counter, estimator_type, quality_percent
  fields instead of hardcoded 0/MAV_ESTIMATOR_TYPE_MOCAP/0

For backward compatibility, estimator_type defaults to
MAV_ESTIMATOR_TYPE_MOCAP when set to Unknown.

Co-authored-by: Julian Oes <julian@oes.ch>
@julianoes julianoes force-pushed the mocap-wire-up-new-fields branch from f2b9178 to 45b295a Compare April 1, 2026 01:40
@julianoes julianoes merged commit 6a459f2 into mavlink:main Apr 1, 2026
55 of 56 checks passed
bansiesta pushed a commit to bansiesta/MAVSDK that referenced this pull request Apr 1, 2026
bansiesta pushed a commit to bansiesta/MAVSDK that referenced this pull request Apr 20, 2026
…oto fields (mavlink#2791)

Replace hardcoded values with the corresponding proto fields added
in mavlink/MAVSDK-Proto#395:

- send_vision_position_estimate: use reset_counter field instead of
  hardcoded 0

- send_vision_speed_estimate: use reset_counter field instead of
  hardcoded 0

- send_odometry: use reset_counter, estimator_type, quality_percent
  fields instead of hardcoded 0/MAV_ESTIMATOR_TYPE_MOCAP/0

For backward compatibility, estimator_type defaults to
MAV_ESTIMATOR_TYPE_MOCAP when set to Unknown.

Co-authored-by: alireza787b <alireza787b@gmail.com>
Co-authored-by: Julian Oes <julian@oes.ch>
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.

3 participants