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

Test: added check for quaternion vector to vector rotation constructor

Browse Source 
including all the parallel vector corner cases
This commit is contained in:
Matthias Grob 2017-11-13 17:29:49 +01:00
parent 84cd7483ae
commit af2610ec04
1 changed files with 29 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
test/attitude.cpp
View File

@ -27,10 +27,7 @@ int main()
TEST(isEqual(e, e));
// euler vector ctor
Vector<float, 3> v;
v(0) = 0.1f;
v(1) = 0.2f;
v(2) = 0.3f;
Vector3f v(0.1f, 0.2f, 0.3f);
Eulerf euler_copy(v);
TEST(isEqual(euler_copy, euler_check));
@ -42,6 +39,33 @@ int main()
TEST(fabs(q(2) - 3) < eps);
TEST(fabs(q(3) - 4) < eps);
// quaternion ctor: vector to vector
Vector3f v1(0.f, 0.f, 1.f);
// identity & default destination vector test
Quatf quat_v(v1);
TEST(isEqual(quat_v.conjugate(v1), v1));
// random test (vector norm can not be preserved with a pure rotation)
v1 = Vector3f(-80.1f, 1.5f, -6.89f);
quat_v = Quatf(v1, v);
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
// special 180 degree case 1
v1 = Vector3f(0.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
// special 180 degree case 2
v1 = Vector3f(1.f, 2.f, 0.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
// special 180 degree case 3
v1 = Vector3f(0.f, 0.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
// special 180 degree case 4
v1 = Vector3f(1.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
// quat normalization
q.normalize();
TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f,
@ -297,7 +321,7 @@ int main()
TEST(isEqual(Dcmf(q), q.to_dcm()));
// conjugate
Vector3f v1(1.5f, 2.2f, 3.2f);
v = Vector3f(1.5f, 2.2f, 3.2f);
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));