[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()