From d9688fae7d1712fed290485a62a7327940bee971 Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 25 Oct 2016 15:07:31 +0200 Subject: [PATCH] EKF: Simplify terrain position validity check Remove the relative accuracy check that was false triggering close to ground --- EKF/terrain_estimator.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index b337005174..5be79c3b3f 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -135,10 +135,12 @@ bool Ekf::get_terrain_vert_pos(float *ret) { memcpy(ret, &_terrain_vpos, sizeof(float)); - // The height is useful if the uncertainty in terrain height is significantly smaller than than the estimated height above terrain - bool accuracy_useful = (sqrtf(_terrain_var) < 0.2f * fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance)); + if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < 1e6)) { + return true; - return (_time_last_imu - _time_last_hagl_fuse < 1e6) || accuracy_useful; + } else { + return false; + } } void Ekf::get_hagl_innov(float *hagl_innov)