mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 00:09:07 +08:00
Added more warnings, and fixed issues.
This commit is contained in:
parent
6ba15eeaa0
commit
0062cbcbe2
@ -16,31 +16,31 @@ int main()
|
||||
double eps = 1e-6;
|
||||
|
||||
// check data
|
||||
Eulerf euler_check(0.1, 0.2, 0.3);
|
||||
Quatf q_check(0.98334744, 0.0342708, 0.10602051, .14357218);
|
||||
Eulerf euler_check(0.1f, 0.2f, 0.3f);
|
||||
Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
|
||||
float dcm_data[] = {
|
||||
0.93629336, -0.27509585, 0.21835066,
|
||||
0.28962948, 0.95642509, -0.03695701,
|
||||
-0.19866933, 0.0978434 , 0.97517033
|
||||
0.93629336f, -0.27509585f, 0.21835066f,
|
||||
0.28962948f, 0.95642509f, -0.03695701f,
|
||||
-0.19866933f, 0.0978434f, 0.97517033f
|
||||
};
|
||||
Dcmf dcm_check(dcm_data);
|
||||
|
||||
// euler ctor
|
||||
euler_check.T().print();
|
||||
assert((euler_check - Vector3f(0.1, 0.2, 0.3)).norm() < eps);
|
||||
assert((euler_check - Vector3f(0.1f, 0.2f, 0.3f)).norm() < eps);
|
||||
|
||||
// quaternion ctor
|
||||
Quatf q(1, 2, 3, 4);
|
||||
assert(q(0) == 1);
|
||||
assert(q(1) == 2);
|
||||
assert(q(2) == 3);
|
||||
assert(q(3) == 4);
|
||||
assert(fabs(q(0) - 1) < eps);
|
||||
assert(fabs(q(1) - 2) < eps);
|
||||
assert(fabs(q(2) - 3) < eps);
|
||||
assert(fabs(q(3) - 4) < eps);
|
||||
|
||||
q.T().print();
|
||||
q.normalize();
|
||||
q.T().print();
|
||||
assert((q - Quatf(
|
||||
0.18257419, 0.36514837, 0.54772256, 0.73029674)
|
||||
0.18257419f, 0.36514837f, 0.54772256f, 0.73029674f)
|
||||
).norm() < eps);
|
||||
|
||||
// euler to quaternion
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user