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

ekf2: utils add getEulerYaw() that uses the best rotation sequence

Browse Source
This commit is contained in:
Daniel Agar
2022-02-02 08:54:03 -05:00
parent c3e0b93fc8
commit 88ffc177f7
9 changed files with 24 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+1 -3
View File
@@ -192,9 +192,7 @@ bool Ekf::isYawFailure() const
return false;
}
const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
? getEuler321Yaw(_R_to_earth)
: getEuler312Yaw(_R_to_earth);
const float euler_yaw = getEulerYaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);