diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 53c3b74e6f..abb65fc889 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -90,7 +90,8 @@ void Ekf::runTerrainEstimator() // Perform a continuity check on range finder data checkRangeDataContinuity(); - // Perform initialisation check + // Perform initialisation check and + // on ground, continuously reset the terrain estimator if (!_terrain_initialised || !_control_status.flags.in_air) { _terrain_initialised = initHagl(); @@ -108,15 +109,6 @@ 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 or bad range data for over a second, fake a measurement - // to handle bad range finder data when on ground - 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; - } - // Fuse range finder data if available if (_range_data_ready && !_rng_hgt_faulty) { fuseHagl();