Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Commit

Permalink
EKF: Fix bug causing continual yaw reset when EKF2_MAG_TYPE = 2
Browse files Browse the repository at this point in the history
  • Loading branch information
priseborough committed Apr 7, 2019
1 parent f95cd4b commit 43b5e26
Showing 1 changed file with 3 additions and 9 deletions.
12 changes: 3 additions & 9 deletions EKF/control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1533,16 +1533,10 @@ void Ekf::controlMagFusion()
}

} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D || !_control_status.flags.mag_align_complete) {
_control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete;
if (!_control_status.flags.mag_3D && _control_status.flags.yaw_align && (_flt_mag_align_start_time > 0)) {
// only commence 3-axis fusion when yaw is aligned and field states set
_control_status.flags.mag_3D = true;
}

// use 3-axis mag fusion if reset was successful
_control_status.flags.mag_3D = _control_status.flags.mag_align_complete;
_control_status.flags.mag_hdg = false;

} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
Expand Down

0 comments on commit 43b5e26

Please sign in to comment.