fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 14:51:37 +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-05-26 08:48:08 -06:00
parent bd182ecf70
commit 6535cc758e
1 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_fusion.cpp
+6
View File
@@ -167,6 +167,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
} else if (index == 2) { } else if (index == 2) {
// we do not fuse synthesized magnetomter measurements when doing 3D fusion // we do not fuse synthesized magnetomter measurements when doing 3D fusion
if (_control_status.flags.synthetic_mag_z) { if (_control_status.flags.synthetic_mag_z) {
fused[2] = true;
continue; 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]) { if (fused[0] && fused[1] && fused[2]) {
aid_src_mag.fused = true; aid_src_mag.fused = true;
aid_src_mag.time_last_fuse = _time_delayed_us; aid_src_mag.time_last_fuse = _time_delayed_us;
if (update_all_states) {
_time_last_heading_fuse = _time_delayed_us;
}
return true; return true;
} }