diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 9ba6db7242..3cb1c76080 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -95,9 +95,9 @@ void Ekf::controlHaglRngFusion() } if (_range_sensor.isDataHealthy()) { - const bool continuing_conditions_passing = _control_status.flags.in_air; + const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent(); //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height - const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData(); + const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); _time_last_healthy_rng_data = _time_last_imu;