From d579fb540c58a9a8e34d79ec9d2cf6e8492b1758 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 30 Oct 2024 11:51:24 +0100 Subject: [PATCH] ekf2-terrain: do not consider negative hagl as valid --- src/modules/ekf2/EKF/terrain_control.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 82f842217628..323d5ac55349 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -79,7 +79,8 @@ void Ekf::updateTerrainValidity() { bool valid_opt_flow_terrain = false; bool valid_rng_terrain = false; - bool valid_hagl_var = false; + bool positive_hagl_var = false; + bool small_relative_hagl_var = false; #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -106,21 +107,29 @@ void Ekf::updateTerrainValidity() float hagl_var = INFINITY; sym::ComputeHaglInnovVar(P, 0.f, &hagl_var); - if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) { - valid_hagl_var = true; + positive_hagl_var = hagl_var > 0.f; + + if (positive_hagl_var + && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f))) + ) { + small_relative_hagl_var = true; } } + const bool positive_hagl = getHagl() >= 0.f; + 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) + if (positive_hagl + && positive_hagl_var + && (valid_rng_terrain + || (valid_opt_flow_terrain && small_relative_hagl_var)) ) { _terrain_valid = true; } } else { // terrain was previously valid, continue considering valid if variance is good - _terrain_valid = valid_hagl_var; + _terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var; } }