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

EKF: Fix bug causing continual yaw reset when EKF2_MAG_TYPE = 2

Browse Source
This commit is contained in:
Paul Riseborough
2019-03-02 09:03:56 +11:00
committed by Paul Riseborough
parent f95cd4b358
commit 43b5e26fe4
1 changed files with 3 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -9
View File
@@ -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;