[1.14] Fix terrain estimator validity issue

src/modules/ekf2/EKF/range_finder_consistency_check.cpp

@@ -37,8 +37,12 @@
 
 #include "range_finder_consistency_check.hpp"
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
 {
+	if (horizontal_motion) {
+		_time_last_horizontal_motion = time_us;
+	}
+
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
 	if ((_time_last_update_us == 0)
@@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 
 void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 {
+	if (fabsf(vz) < _min_vz_for_valid_consistency) {
+		// We can only check consistency if there is vertical motion
+		return;
+	}
+
 	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		_is_kinematically_consistent = false;
-		_time_last_inconsistent_us = time_us;
+		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
+			_is_kinematically_consistent = false;
+			_time_last_inconsistent_us = time_us;
+		}
 
 	} else {
-		if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if (_test_ratio < 1.f
+		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
 			_is_kinematically_consistent = true;
 		}
 	}

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -47,7 +47,7 @@ class RangeFinderConsistencyCheck final
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
 
 	void setGate(float gate) { _gate = gate; }
 
@@ -71,6 +71,7 @@ class RangeFinderConsistencyCheck final
 
 	bool _is_kinematically_consistent{true};
 	uint64_t _time_last_inconsistent_us{};
+	uint64_t _time_last_horizontal_motion{};
 
 	static constexpr float _signed_test_ratio_tau = 2.f;
 

src/modules/ekf2/EKF/range_height_control.cpp

@@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-			// Run the kinematic consistency check when not moving horizontally
-			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
-			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
+			if (_control_status.flags.in_air) {
+				const bool horizontal_motion = _control_status.flags.fixed_wing
+								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f));
 
 				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 				const float var = sq(_params.range_noise) + dist_dependant_var;
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us);
 			}
 
 		} else {

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -199,7 +199,6 @@ void Ekf::resetHaglRng()
 	_terrain_var = getRngVar();
 	_terrain_vpos_reset_counter++;
 	_time_last_hagl_fuse = _time_delayed_us;
-	_time_last_healthy_rng_data = 0;
 }
 
 void Ekf::stopHaglRngFusion()