fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

add odometry reset counter (ekf2, mavlink, etc)

Browse Source
This commit is contained in:
Daniel Agar
2022-01-31 09:19:36 -05:00
parent 463ac8e8a1
commit 90358f078f
12 changed files with 69 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+4
View File
@@ -1048,6 +1048,10 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
odom.reset_counter = _ekf.get_quat_reset_count()
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
+ _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
// publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_odometry_pub.publish(odom);