diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ceefc01f6f..8cb9e4be67 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -146,9 +146,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(imu_delayed); #endif // CONFIG_EKF2_AUXVEL - // + #if defined(CONFIG_EKF2_TERRAIN) controlTerrainFakeFusion(); + updateTerrainValidity(); #endif // CONFIG_EKF2_TERRAIN controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 35c564157f..3192aaf214 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -99,7 +99,7 @@ public: #if defined(CONFIG_EKF2_TERRAIN) // terrain estimate - bool isTerrainEstimateValid() const; + bool isTerrainEstimateValid() const { return _terrain_valid; } // get the estimated terrain vertical position relative to the NED origin float getTerrainVertPos() const { return _state.terrain; }; @@ -500,6 +500,8 @@ private: #if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) + + bool _terrain_valid{false}; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -733,6 +735,8 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; } void controlTerrainFakeFusion(); + void updateTerrainValidity(); + # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain); diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 61356b1a41..82f8422176 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -75,9 +75,31 @@ void Ekf::controlTerrainFakeFusion() } } -bool Ekf::isTerrainEstimateValid() const +void Ekf::updateTerrainValidity() { - bool valid = false; + bool valid_opt_flow_terrain = false; + bool valid_rng_terrain = false; + bool valid_hagl_var = false; + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_control_status.flags.opt_flow_terrain + && isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max) + ) { + valid_opt_flow_terrain = true; + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_control_status.flags.rng_terrain + && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max) + ) { + valid_rng_terrain = true; + } + +#endif // CONFIG_EKF2_RANGE_FINDER if (_time_last_terrain_fuse != 0) { // Assume being valid when the uncertainty is small compared to the height above ground @@ -85,18 +107,20 @@ bool Ekf::isTerrainEstimateValid() const sym::ComputeHaglInnovVar(P, 0.f, &hagl_var); if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) { - valid = true; + valid_hagl_var = true; } } -#if defined(CONFIG_EKF2_RANGE_FINDER) + if (!_terrain_valid) { + // require valid RNG or optical flow (+valid variance) to initially consider terrain valid + if (valid_rng_terrain + || (valid_opt_flow_terrain && valid_hagl_var) + ) { + _terrain_valid = true; + } - // Assume that the terrain estimate is always valid when direct observations are fused - if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)) { - valid = true; + } else { + // terrain was previously valid, continue considering valid if variance is good + _terrain_valid = valid_hagl_var; } - -#endif // CONFIG_EKF2_RANGE_FINDER - - return valid; }