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

Use baro if reset to baro from gps and vice versa

Browse Source
This commit is contained in:
ChristophTobler 2017-06-29 17:07:42 +02:00
parent 4a4b0fa604
commit 0d9e535acb
1 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
EKF/control.cpp
View File

@ -740,6 +740,16 @@ void Ekf::controlHeightFusion()
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
// switch to gps if there was a reset to gps
_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
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
}
}
}
@ -797,6 +807,16 @@ void Ekf::controlHeightFusion()
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
}
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_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
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
}