fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 18:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix vulnerability to ground level mag anomally when using heading fusion (#544)

Browse Source 
Previously, the reset of the yaw when climbing above 1.5m was not performed until 3-axis fusion was enabled. This could result in loss of navigation depending on the value of  EKF2_MAG_TYPE and the flight profile.
This commit is contained in:
Paul Riseborough
2019-01-03 09:41:55 -05:00
committed by Daniel Agar
parent 4822bf8386
commit 48d83f3bcb
1 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -1
View File
@@ -1356,10 +1356,19 @@ void Ekf::controlMagFusion()
// check for new magnetometer data that has fallen behind the fusion time horizon // check for new magnetometer data that has fallen behind the fusion time horizon
// If we are using external vision data for heading then no magnetometer fusion is used // If we are using external vision data for heading then no magnetometer fusion is used
if (!_control_status.flags.ev_yaw && _mag_data_ready) { if (!_control_status.flags.ev_yaw && _mag_data_ready) {
// We need to reset the yaw angle after climbing away from the ground to enable
// recovery from ground level magnetic interference.
if (!_flt_mag_align_complete) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested.
_mag_yaw_reset_req |= (_last_on_ground_posD - _state.pos(2)) > 1.5f;
}
// perform a yaw reset if requested by other functions // perform a yaw reset if requested by other functions
if (_mag_yaw_reset_req) { if (_mag_yaw_reset_req) {
if (!_mag_use_inhibit ) { if (!_mag_use_inhibit ) {
resetMagHeading(_mag_sample_delayed.mag); _flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air;
} }
_mag_yaw_reset_req = false; _mag_yaw_reset_req = false;
} }