diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index c6e0504f15..527f50e306 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -35,7 +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 +bool cs_rng_kin_valid # 31 - true when the range finder data is healthy and the kinematic consistency check is passing bool cs_fake_pos # 32 - true when fake position measurements are being fused bool cs_fake_hgt # 33 - true when fake height measurements are being fused bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 8b4b96712d..ffb832014b 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -86,7 +86,9 @@ void Ekf::controlRangeHeightFusion() } } - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + if (_range_sensor.isDataReady()) { + _control_status.flags.rng_kin_valid = _rng_consistency_check.isKinematicallyConsistent() && _range_sensor.isHealthy(); + } } else { return; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 8624501cb2..4e9e3deecc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -578,7 +578,7 @@ union filter_control_status_u { uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements - uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing + uint64_t rng_kin_valid : 1; ///< 31 - true when the range finder data is healthy and the kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7cfefd9d2c..09003e5032 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1908,7 +1908,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.cs_rng_kin_valid = _ekf.control_status_flags().rng_kin_valid; status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt; status_flags.cs_gravity_vector = _ekf.control_status_flags().gravity_vector;