fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 12:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Compensate for bad range finder data when on ground

Browse Source
This commit is contained in:
Paul Riseborough
2019-09-11 09:55:16 +10:00
committed by Paul Riseborough
parent 3b32ee4166
commit 3e05fd552e
1 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+7
View File
@@ -97,6 +97,13 @@ 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
// 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) {
_range_data_ready = true;
_range_sample_delayed.rng = _params.rng_gnd_clearance;
}
// Fuse range finder data if available
if (_range_data_ready && !_rng_hgt_faulty) {
fuseHagl();