dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: estimator_status log reset counts

This commit is contained in:
Daniel Agar 2021-02-17 21:16:34 -05:00 committed by GitHub
parent 6d42fe28bf
commit cff7a01d5e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/estimator_status.msg
View File

@ -105,6 +105,12 @@ uint16 solution_status_flags # Bitmask indicating which filter kinematic state o
# 10 - True if the EKF has detected a GPS glitch
# 11 - True if the EKF has detected bad accelerometer data
uint8 reset_count_vel_ne # number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8 reset_count_vel_d # number of vertical velocity reset events (allow to wrap if count exceeds 255)
uint8 reset_count_pos_ne # number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8 reset_count_pod_d # number of vertical position reset events (allow to wrap if count exceeds 255)
uint8 reset_count_quat # number of quaternion reset events (allow to wrap if count exceeds 255)
float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
bool pre_flt_fail_innov_heading

7
src/modules/ekf2/EKF2.cpp
View File

@ -966,6 +966,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.getImuVibrationMetrics().copyTo(status.vibe);
// reset counters
status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
status.reset_count_quat = _ekf.state_reset_status().quat_counter;
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();