fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 16: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/ekf_helper.cpp
+5
View File
@@ -437,6 +437,8 @@ bool Ekf::realignYawGPS()
P[index][index] = sq(_params.mag_noise);
}
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
@@ -637,6 +639,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
P[index][index] = sq(_params.mag_noise);
}
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;