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

range aid: switch to baro/gps even if rng aid conditions met and rng faulty

Browse Source
This commit is contained in:
ChristophTobler
2017-06-29 09:47:02 +02:00
committed by ChristophTobler
parent 2f382c8e0e
commit 066c641d61
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+6 -2
View File
@@ -730,9 +730,11 @@ void Ekf::controlHeightFusion()
}
}
} else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
} else if (_baro_data_ready && !_baro_hgt_faulty &&
!(_in_range_aid_mode && !_range_data_ready && !_rng_hgt_faulty)) {
setControlBaroHeight();
_fuse_height = true;
_in_range_aid_mode = false;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
@@ -785,9 +787,11 @@ void Ekf::controlHeightFusion()
}
}
} else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {
} else if (_gps_data_ready && !_gps_hgt_faulty &&
!(_in_range_aid_mode && !_range_data_ready && !_rng_hgt_faulty)) {
setControlGPSHeight();
_fuse_height = true;
_in_range_aid_mode = false;
// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate