fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 07:33: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/utils.cpp
+1 -16
View File
@@ -32,7 +32,7 @@ float kahanSummation(float sum_previous, float input, float &accumulator)
return t;
}
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
{
const float q00 = quat(0) * quat(0);
const float q11 = quat(1) * quat(1);
@@ -59,11 +59,6 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
return dcm;
}
bool shouldUse321RotationSequence(const matrix::Dcmf &R)
{
return fabsf(R(2, 0)) < fabsf(R(2, 1));
}
float getEuler321Yaw(const matrix::Quatf &q)
{
// Values from yaw_input_321.c file produced by
@@ -73,11 +68,6 @@ float getEuler321Yaw(const matrix::Quatf &q)
return atan2f(a, b);
}
float getEuler321Yaw(const matrix::Dcmf &R)
{
return atan2f(R(1, 0), R(0, 0));
}
float getEuler312Yaw(const matrix::Quatf &q)
{
// Values from yaw_input_312.c file produced by
@@ -87,11 +77,6 @@ float getEuler312Yaw(const matrix::Quatf &q)
return atan2f(a, b);
}
float getEuler312Yaw(const matrix::Dcmf &R)
{
return atan2f(-R(0, 1), R(1, 1));
}
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
{
matrix::Eulerf euler321(rot_in);