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

Check for stuck range finder measurements in terrain estimator

This commit is contained in:
ChristophTobler 2017-07-31 17:59:44 +02:00
parent 7252284628
commit 8ecec58292
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

@ -93,7 +93,7 @@ void Ekf::runTerrainEstimator()
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available
if (_range_data_ready) {
if (_range_data_ready && !_rng_stuck) {
fuseHagl();
// update range sensor angle parameters in case they have changed
@ -158,7 +158,8 @@ void Ekf::fuseHagl()
// return true if the terrain estimate is valid
bool Ekf::get_terrain_valid()
{
if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
!_innov_check_fail_status.flags.reject_hagl) {
return true;
} else {