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

EKF: fake rng if faulty on ground

This commit is contained in:
Nico van Duijn 2019-09-12 17:22:52 +02:00 committed by Paul Riseborough
parent 4f19d457c4
commit c446ee444a
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/terrain_estimator.cpp
View File

@ -97,9 +97,10 @@ 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
// 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 (!_range_data_ready && !_control_status.flags.in_air && _vehicle_at_rest && (_time_last_imu - _time_last_hagl_fuse) > (uint64_t)1E6) {
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;