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

ekf2: simplify fuseYaw() signature and use consistently

Browse Source 
- make it safe to call for other aid sources, no EKF state is changed unless fusion_enabled
This commit is contained in:
Daniel Agar
2023-07-24 10:34:00 -04:00
committed by GitHub
parent 01bcc47fb1
commit 1ef9ee7622
5 changed files with 83 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+4 -1
View File
@@ -524,9 +524,12 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
_aid_src_mag_heading.observation = measured_hdg;
_aid_src_mag_heading.observation_variance = obs_var;
_aid_src_mag_heading.innovation = innovation;
_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
fuseYaw(innovation, obs_var, _aid_src_mag_heading);
fuseYaw(_aid_src_mag_heading);
}
}