From 0c24aa614936f571ec2c4d1efc794b2bcec4bf2e Mon Sep 17 00:00:00 2001 From: Elia Tarasov Date: Mon, 11 Mar 2019 14:44:01 +0300 Subject: [PATCH] enable ground effect zone when valid terrain estimate is available --- src/modules/ekf2/ekf2_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 20b2bfe68d..bf052d6e44 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1253,10 +1253,6 @@ void Ekf2::run() if (vehicle_land_detected_updated) { if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) { _ekf.set_in_air_status(!vehicle_land_detected.landed); - - if (_gnd_effect_deadzone.get() > 0.0f) { - _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); - } } } @@ -1397,6 +1393,11 @@ void Ekf2::run() lpos.dist_bottom = _rng_gnd_clearance.get(); } + // update ground effect flag based on terrain estimation + if (lpos.dist_bottom_valid && lpos.dist_bottom < _gnd_effect_deadzone.get()) { + _ekf.set_gnd_effect_flag(true); + } + lpos.dist_bottom_rate = -lpos.vz; // Distance to bottom surface (ground) change rate _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);