fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 22:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Allow mag field estimates to stabilise before use

Browse Source
This commit is contained in:
Paul Riseborough
2017-11-13 06:55:02 +11:00
parent 363edf5eb9
commit 29d383edbf
4 changed files with 22 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+9
View File
@@ -1244,6 +1244,9 @@ void Ekf::controlMagFusion()
_control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW)
&& _control_status.flags.fixed_wing;
// For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7);
if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
// When re-commencing use of magnetometer to correct vehicle states
// set the field state variance to the observation variance and zero
@@ -1293,6 +1296,12 @@ void Ekf::controlMagFusion()
fuseDeclination();
}
if (_flt_mag_align_converging) {
_control_status.flags.mag_hdg = true;
fuseHeading();
_control_status.flags.mag_hdg = false;
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();