From 61a7991693bd6a2a339a2894fe4b9fb3acf0c772 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Wed, 19 Jul 2017 11:26:03 +0200 Subject: [PATCH] constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) --- EKF/terrain_estimator.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 78136a1d2e..f21e3cc36a 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -102,6 +102,11 @@ void Ekf::runTerrainEstimator() _cos_tilt_rng = cosf(_params.rng_sens_pitch); } + + //constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) + if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) { + _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); + } } }