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

Use Hamilton quaternion definition.

Browse Source
This commit is contained in:
James Goppert
2016-08-16 03:59:06 -04:00
parent e6a6b4680c
commit 19554c4470
2 changed files with 27 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/attitude.cpp
+13 -7
View File
@@ -156,6 +156,19 @@ int main()
Quatf q_from_m(m4);
TEST(isEqual(q_from_m, m4));
// quaternion product
Quatf q3(0, 0, 0, 1);
Quatf q4(AxisAnglef(0, 0, float(2*M_PI)));
q4 *= q4;
q4 *= q3;
TEST(isEqual(q3, q4));
q3 = Quatf(0, 0, 0, 1);
q4 = AxisAnglef(0, 0, float(M_PI/2));
q4 *= q4;
q4 *= q3;
TEST(isEqual(Quatf(-1, 0, 0, 0), q4));
// quaternion derivate
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
@@ -163,13 +176,6 @@ int main()
Vector<float, 4> q1_dot_check(data_q_dot_check);
TEST(isEqual(q1_dot, q1_dot_check));
// quaternion product
Quatf q_prod_check(
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
TEST(isEqual(q_prod_check, q_check*q_check));
q_check *= q_check;
TEST(isEqual(q_prod_check, q_check));
// Quaternion scalar multiplication
float scalar = 0.5;
Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);