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

ekf2: fuse mag update last heading fuse time if updating all states

Browse Source 
- handle synthetic z special case
This commit is contained in:
Daniel Agar 2023-03-09 10:53:12 -05:00
parent bd182ecf70
commit 6535cc758e
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -167,6 +167,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
} else if (index == 2) {
// we do not fuse synthesized magnetomter measurements when doing 3D fusion
if (_control_status.flags.synthetic_mag_z) {
fused[2] = true;
continue;
}
@ -216,6 +217,11 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
if (fused[0] && fused[1] && fused[2]) {
aid_src_mag.fused = true;
aid_src_mag.time_last_fuse = _time_delayed_us;
if (update_all_states) {
_time_last_heading_fuse = _time_delayed_us;
}
return true;
}