From bc951f6f157246c05c11596c41f2e94d60f178f8 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 21 Jul 2017 23:49:00 +0200 Subject: [PATCH] add constraint to ground distance to avoid values below rng_gnd_clearance (#7662) --- src/modules/ekf2/ekf2_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 381b3b1de0..fcf2e92b57 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -942,6 +942,12 @@ void Ekf2::run() lpos.dist_bottom_valid = _ekf.get_terrain_valid(); _ekf.get_terrain_vert_pos(&terrain_vpos); lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters + + // constrain the distance to ground to _params->rng_gnd_clearance + if (lpos.dist_bottom < _params->rng_gnd_clearance) { + lpos.dist_bottom = _params->rng_gnd_clearance; + } + lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = now; // Time when new bottom surface found