diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 5c3e765b50..7f8dc28cb0 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9d34954b20..5c6f72fbe9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -966,6 +966,13 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _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();