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

Cleanup of printing.

Browse Source
This commit is contained in:
jgoppert
2015-11-05 17:13:03 -05:00
parent 455cb58ebf
commit 75399fb15e
1 changed files with 20 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/attitude.cpp
+20 -7
View File
@@ -96,22 +96,35 @@ int main()
// euler gimbal lock check
// note if theta = pi/2, then roll is set to zero
float pi_2 = float(M_PI_2);
Eulerf euler_gimbal_lock(0.0f, pi_2, 0.0f);
Eulerf euler_gimbal_lock(0.1f, pi_2, 0.2f);
Dcmf dcm_lock(euler_gimbal_lock);
Eulerf euler_gimbal_lock_out(dcm_lock);
euler_gimbal_lock_out.print();
euler_gimbal_lock.print();
euler_gimbal_lock_out.T().print();
euler_gimbal_lock.T().print();
assert(euler_gimbal_lock == euler_gimbal_lock_out);
// note if theta = pi/2, then roll is set to zero
Eulerf euler_gimbal_lock2(0.0f, -pi_2, 0.0f);
Eulerf euler_gimbal_lock2(0.1f, -pi_2, 0.2f);
Dcmf dcm_lock2(euler_gimbal_lock2);
Eulerf euler_gimbal_lock_out2(dcm_lock2);
euler_gimbal_lock_out2.print();
euler_gimbal_lock2.print();
euler_gimbal_lock_out2.T().print();
euler_gimbal_lock2.T().print();
assert(euler_gimbal_lock2 == euler_gimbal_lock_out2);
// quaterion copy ctors
float data_v4[] = {1, 2, 3, 4};
Vector<float, 4> v4(data_v4);
Quatf q_from_v(v4);
assert(q_from_v == v4);
}
Matrix<float, 4, 1> m4(data_v4);
Quatf q_from_m(m4);
assert(q_from_m == m4);
// quaternion derivate
Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
printf("q_dot:\n");
q_dot.T().print();
};
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */