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

ekf2: wrap_pi fixes and fuseYaw() renaming

Browse Source 
- to stay consistent with https://github.com/PX4/PX4-Autopilot/pull/19434
This commit is contained in:
Daniel Agar
2022-04-06 10:50:20 -04:00
parent b8315ea72a
commit 4084beaa53
6 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+3 -3
View File
@@ -327,14 +327,14 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
float measured_hdg = wrap_pi(-atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination());
float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg);
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
// Update the quaternion states and covariance matrix
if (updateQuaternion(innovation, obs_var)) {
if (fuseYaw(innovation, obs_var)) {
_time_last_mag_heading_fuse = _time_last_imu;
}
}