From d903613c9ce203a5207c85041175fc782f80d694 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 21 Mar 2022 15:31:43 +0100 Subject: [PATCH] ekf: add logging for rng kinematic consistency check --- msg/estimator_status_flags.msg | 1 + src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 2 ++ src/modules/ekf2/EKF2.cpp | 1 + 4 files changed, 5 insertions(+) diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 232f22af1d..5176db9af5 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -35,6 +35,7 @@ bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declare bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements +bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index dc67b7c527..a964e2ea21 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -493,6 +493,7 @@ union filter_control_status_u { uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift uint32_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements + uint32_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 07185442d7..6a1f5d95a3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -142,6 +142,8 @@ void Ekf::controlFusionModes() _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu); } + + _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); } if (_flow_buffer) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 206863f78d..6a300f1f2d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1304,6 +1304,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault; status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning; status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning; + status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;