dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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-04 01:41:55 +11:00 committed by Daniel Agar
parent 4822bf8386
commit 48d83f3bcb
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/control.cpp
View File

@ -1356,10 +1356,19 @@ void Ekf::controlMagFusion()
// 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 (!_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
if (_mag_yaw_reset_req) {
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;
}