From c446ee444ab6c32bb61fafe4ba32f1cc7f3e04f3 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Thu, 12 Sep 2019 17:22:52 +0200 Subject: [PATCH] EKF: fake rng if faulty on ground --- EKF/terrain_estimator.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 929258d0ab..9b64a5902d 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -97,9 +97,10 @@ void Ekf::runTerrainEstimator() // limit the variance to prevent it becoming badly conditioned _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); - // if stationary on the ground and no range data for over a second, fake a measurement + // if stationary on the ground and no or bad range data for over a second, fake a measurement // to handle bad range finder data when on ground - if (!_range_data_ready && !_control_status.flags.in_air && _vehicle_at_rest && (_time_last_imu - _time_last_hagl_fuse) > (uint64_t)1E6) { + if ((_rng_hgt_faulty || !_range_data_ready) && !_control_status.flags.in_air && _vehicle_at_rest && + (_time_last_imu - _time_last_hagl_fuse) > (uint64_t)1E6) { _range_data_ready = true; _rng_hgt_faulty = false; _range_sample_delayed.rng = _params.rng_gnd_clearance;