dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2019-10-10 14:51:29 +02:00 committed by Paul Riseborough
parent 370e04ee60
commit 0aef0eda59
1 changed files with 2 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/terrain_estimator.cpp
View File

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