fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 19:13:33 +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/utils.hpp
+14 -4
View File
@@ -27,13 +27,23 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
// when there is more pitch than roll tilt to avoid gimbal lock.
bool shouldUse321RotationSequence(const matrix::Dcmf &R);
inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); }
inline float getEuler321Yaw(const matrix::Dcmf &R) { return atan2f(R(1, 0), R(0, 0)); }
inline float getEuler312Yaw(const matrix::Dcmf &R) { return atan2f(-R(0, 1), R(1, 1)); }
float getEuler321Yaw(const matrix::Quatf &q);
float getEuler321Yaw(const matrix::Dcmf &R);
float getEuler312Yaw(const matrix::Quatf &q);
float getEuler312Yaw(const matrix::Dcmf &R);
inline float getEulerYaw(const matrix::Dcmf &R)
{
if (shouldUse321RotationSequence(R)) {
return getEuler321Yaw(R);
} else {
return getEuler312Yaw(R);
}
}
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in);
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in);