fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 16:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: range finder aiding logic fixes

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-03 11:32:26 +10:00
parent 0c0a6602b0
commit 24b005ed57
1 changed files with 2 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -7
View File
@@ -882,10 +882,9 @@ void Ekf::controlHeightFusion()
} }
} }
} else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) { } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight(); setControlBaroHeight();
_fuse_height = true; _fuse_height = true;
_range_aid_mode_enabled = false;
// we have just switched to using baro height, we don't need to set a height sensor offset // 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 // since we track a separate _baro_hgt_offset
@@ -907,7 +906,6 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) { } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
// switch to gps if there was a reset to gps // switch to gps if there was a reset to gps
_fuse_height = true; _fuse_height = true;
_range_aid_mode_enabled = false;
// we have just switched to using gps height, calculate height sensor offset such that current // we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate // measurment matches our current height estimate
@@ -969,10 +967,9 @@ void Ekf::controlHeightFusion()
} }
} }
} else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) { } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) {
setControlGPSHeight(); setControlGPSHeight();
_fuse_height = true; _fuse_height = true;
_range_aid_mode_enabled = false;
// we have just switched to using gps height, calculate height sensor offset such that current // we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate // measurment matches our current height estimate
@@ -983,7 +980,6 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro // switch to baro if there was a reset to baro
_fuse_height = true; _fuse_height = true;
_range_aid_mode_enabled = false;
// we have just switched to using baro height, we don't need to set a height sensor offset // 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 // since we track a separate _baro_hgt_offset
@@ -998,7 +994,6 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro // switch to baro if there was a reset to baro
_fuse_height = true; _fuse_height = true;
_range_aid_mode_enabled = false;
// we have just switched to using baro height, we don't need to set a height sensor offset // 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 // since we track a separate _baro_hgt_offset