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

if hgt mode is range, switch to baro if range is faulty

Browse Source
This commit is contained in:
ChristophTobler
2017-06-23 09:53:22 +02:00
committed by ChristophTobler
parent e4f36215cb
commit 2f382c8e0e
1 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+19
View File
@@ -746,6 +746,25 @@ void Ekf::controlHeightFusion()
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
}
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
// 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
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
// Determine if GPS should be used as the height source