|
|
|
@ -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(); |
|
|
|
|