Skip to content

mocap: add reset_counter, quality, estimator_type to odometry messages#395

Merged
julianoes merged 2 commits intomavlink:mainfrom
alireza787b:mocap-add-odometry-fields
Mar 29, 2026
Merged

mocap: add reset_counter, quality, estimator_type to odometry messages#395
julianoes merged 2 commits intomavlink:mainfrom
alireza787b:mocap-add-odometry-fields

Conversation

@alireza787b
Copy link
Copy Markdown
Contributor

@alireza787b alireza787b commented Mar 10, 2026

Problem

The Mocap plugin's Odometry and VisionPositionEstimate messages are missing
fields that PX4's EKF2 can use for better sensor fusion. In the C++ implementation
(mocap_impl.cpp), these values are hardcoded:

  • reset_counter: Hardcoded to 0 with // FIXME: reset_counter not set comments.
    EKF2 uses this to detect when the external estimator has re-initialized, so it can
    perform its own state reset rather than trying to fuse a discontinuous measurement.

  • estimator_type: Hardcoded to MAV_ESTIMATOR_TYPE_MOCAP. For VIO users, this
    should be MAV_ESTIMATOR_TYPE_VIO. PX4's EKF2 can use this to select appropriate
    noise models.

  • quality: Hardcoded to 0. PX4 can scale measurement noise based on this
    confidence metric — low quality = high noise = less influence on state estimate.

Changes

  • Add MavEstimatorType enum (nested in Odometry) matching MAVLink's MAV_ESTIMATOR_TYPE
  • Add reset_counter (field 9), estimator_type (field 10), quality (field 11) to Odometry
  • Add reset_counter (field 5) to VisionPositionEstimate

Backward Compatibility

All new fields use proto3 defaults (0), preserving existing behavior:

  • reset_counter = 0 → same as current (no resets reported)
  • estimator_type = UNKNOWN (0) → C++ impl can default to MOCAP for backward compat
  • quality = 0 → PX4 already handles 0 as "unknown quality"

This is a non-breaking, additive change. Existing users are unaffected.

Related PRs

Add fields that PX4's EKF2 uses for better sensor fusion but were
previously hardcoded in the C++ implementation:

- reset_counter: EKF2 uses this to detect estimator re-initialization
  and perform state resets (currently hardcoded to 0, with FIXME in C++)
- estimator_type: Allows distinguishing VIO from MOCAP estimators
  (currently hardcoded to MAV_ESTIMATOR_TYPE_MOCAP)
- quality: Confidence metric (0-100%) that EKF2 can use to scale
  measurement noise (currently hardcoded to 0)

Also adds MavEstimatorType enum matching MAVLink's MAV_ESTIMATOR_TYPE,
and reset_counter to VisionPositionEstimate.

All new fields use proto3 defaults (0), preserving backward compatibility.
alireza787b pushed a commit to alireza787b/MAVSDK that referenced this pull request Mar 10, 2026
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 (resolves the FIXME comment)

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

Also fixes frame_id mapping: the proto MavFrame enum uses values that
don't match MAVLink (0 instead of 14 for MOCAP_NED), so add a switch
to map to correct MAVLink MAV_FRAME values. For backward compatibility,
estimator_type defaults to MAV_ESTIMATOR_TYPE_MOCAP when set to UNKNOWN.
alireza787b pushed a commit to alireza787b/MAVSDK that referenced this pull request Mar 10, 2026
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 (resolves the FIXME comment)

- send_vision_speed_estimate: use reset_counter field instead of
  hardcoded 0 (resolves the FIXME comment)

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

Also fixes frame_id mapping: the proto MavFrame enum uses values that
don't match MAVLink (0 instead of 14 for MOCAP_NED), so add a switch
to map to correct MAVLink MAV_FRAME values. For backward compatibility,
estimator_type defaults to MAV_ESTIMATOR_TYPE_MOCAP when set to UNKNOWN.
Copy link
Copy Markdown
Collaborator

@JonasVautherin JonasVautherin left a comment

Choose a reason for hiding this comment

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

Thanks for the contribution! Looks fine to me, though I'm not knowledgeable regarding the mocap stuff. I'll let @julianoes be the judge of that 😇

Comment thread protos/mocap/mocap.proto Outdated
Covariance velocity_covariance = 8; // Velocity cross-covariance matrix.
uint32 reset_counter = 9; // Estimate reset counter, for PX4 EKF2 fusion reset detection.
MavEstimatorType estimator_type = 10; // Type of estimator (defaults to UNKNOWN, C++ impl falls back to MOCAP).
int32 quality = 11; // Estimate quality in percent (0-100), -1 for invalid/unknown.
Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

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

I think we usually add the unit to the name:

Suggested change
int32 quality = 11; // Estimate quality in percent (0-100), -1 for invalid/unknown.
int32 quality_percent = 11; // Estimate quality in percent (0-100), -1 for invalid/unknown.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Good catch, thanks Jonas! Renamed to quality_percent (int32, -1 for unknown). Updated in the latest push.

Apply review feedback: rename quality to quality_percent with unit in
name, change type to int32 so -1 can indicate invalid/unknown.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
@julianoes julianoes merged commit 8c68337 into mavlink:main Mar 29, 2026
3 checks passed
@julianoes
Copy link
Copy Markdown
Collaborator

I tried this, and it doesn't build at all:

/home/julianoes/src/upstream/MAVSDK/cpp/src/mavsdk/plugins/mocap/mocap_impl.cpp: In lambda function:
/home/julianoes/src/upstream/MAVSDK/cpp/src/mavsdk/plugins/mocap/mocap_impl.cpp:170:35: error: ‘const struct mavsdk::Mocap::VisionSpeedEstimate’ has no member named ‘reset_counter’
  170 |             vision_speed_estimate.reset_counter);
      |                                   ^~~~~~~~~~~~~
/home/julianoes/src/upstream/MAVSDK/cpp/src/mavsdk/plugins/mocap/mocap_impl.cpp: In member function ‘mavsdk::Mocap::Result mavsdk::MocapImpl::send_odometry(const mavsdk::Mocap::Odometry&)’:
/home/julianoes/src/upstream/MAVSDK/cpp/src/mavsdk/plugins/mocap/mocap_impl.cpp:289:25: error: ‘MAV_FRAME_MOCAP_NED’ was not declared in this scope; did you mean ‘MAV_FRAME_LOCAL_NED’?
  289 |             mav_frame = MAV_FRAME_MOCAP_NED;
      |                         ^~~~~~~~~~~~~~~~~~~
      |                         MAV_FRAME_LOCAL_NED
/home/julianoes/src/upstream/MAVSDK/cpp/src/mavsdk/plugins/mocap/mocap_impl.cpp:301:79: warning: enumerated and non-enumerated type in conditional expression [-Wextra]
  301 |         odometry.estimator_type == Mocap::Odometry::MavEstimatorType::Unknown ?
      |         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
  302 |             MAV_ESTIMATOR_TYPE_MOCAP :
      |             ~~~~~~~~~~~~~~~~~~~~~~~~~~                                         
  303 |             static_cast<uint8_t>(odometry.estimator_type);
      |             ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                      
ninja: build stopped: subcommand failed.

I'm not sure what to do with the frame stuff, especially because you are using MAV_FRAME_MOCAP_NED which has been deprecated according to https://mavlink.io/en/messages/common.html#MAV_FRAME_RESERVED_14.

I'm reverting this one and leave this to you to try to fix, given you have the context.

julianoes added a commit that referenced this pull request Mar 29, 2026
julianoes added a commit that referenced this pull request Mar 29, 2026
julianoes added a commit to alireza787b/MAVSDK that referenced this pull request Apr 1, 2026
…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 added a commit to alireza787b/MAVSDK that referenced this pull request Apr 1, 2026
…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 added a commit to mavlink/MAVSDK that referenced this pull request Apr 1, 2026
…oto fields (#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