Browse Source

terrain_estimator: remove dead code. Since the terrain estimator is

constantly reset on ground, it is not necessary anymore to fuse fake
measurements on ground if the range measurements are bad.
master
bresch 5 years ago committed by Paul Riseborough
parent
commit
0aef0eda59
  1. 12
      EKF/terrain_estimator.cpp

12
EKF/terrain_estimator.cpp

@ -90,7 +90,8 @@ void Ekf::runTerrainEstimator() @@ -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() @@ -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();

Loading…
Cancel
Save