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

add functions to compute yaw (321 and 312 sequence)

Browse Source 
from quaternion and rotation matrix
This commit is contained in:
Kamil Ritz
2020-08-15 13:52:56 +02:00
committed by Paul Riseborough
parent 15afa8ae17
commit fdc86c247a
9 changed files with 60 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_utils.cpp
+13
View File
@@ -55,3 +55,16 @@ TEST(eclPowfTest, compareToStandardImplementation)
}
}
}
TEST(euler312YawTest, fromQuaternion)
{
matrix::Quatf q1(3.5f,2.4f,-0.5f,-3.f);
q1.normalize();
const matrix::Eulerf euler1(q1);
EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1));
matrix::Quatf q2(0.f,0,-1.f,0.f);
q2.normalize();
const matrix::Eulerf euler2(q2);
EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
}