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

More work on gimbal lock case.

Browse Source
This commit is contained in:
jgoppert 2015-11-05 16:54:19 -05:00
parent d01e0a1038
commit 7136e5b7d1
3 changed files with 45 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
matrix/Euler.hpp
View File

@ -49,19 +49,22 @@ public:
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
theta() = Type(asin(-dcm(2, 0)));
Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0)));
Type theta_val = Type(asin(-dcm(2,0)));
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
if (fabs(theta() - M_PI_2) < 1.0e-3) {
psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)));
} else if (fabs(theta() + M_PI_2) < 1.0e-3) {
psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)));
} else {
phi() = Type(atan2f(dcm(2, 1), dcm(2, 2)));
psi() = Type(atan2f(dcm(1, 0), dcm(0, 0)));
// protection against gimbal lock
psi() = 0;
theta() = 0;
phi() = 0;
if (isfinite(psi_val)) {
psi() = psi_val;
}
if (isfinite(theta_val)) {
theta() = theta_val;
}
if (isfinite(phi_val)) {
phi() = phi_val;
}
}

4
matrix/Matrix.hpp
View File

@ -215,9 +215,9 @@ public:
* Misc. Functions
*/
void print()
void print() const
{
Matrix<Type, M, N> &self = *this;
const Matrix<Type, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {

28
test/attitude.cpp
View File

@ -33,6 +33,14 @@ int main()
assert(e == e_zero);
assert(e == e);
// euler vector ctor
Vector<float, 3> v;
v(0) = 0.1f;
v(1) = 0.2f;
v(2) = 0.3f;
Eulerf euler_copy(v);
assert(euler_copy == euler_check);
// quaternion ctor
Quatf q(1, 2, 3, 4);
assert(fabs(q(0) - 1) < eps);
@ -84,6 +92,26 @@ int main()
Quatf q2(dcm_check);
assert(q2 == q_check);
// 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);
Dcmf dcm_lock(euler_gimbal_lock);
Eulerf euler_gimbal_lock_out(dcm_lock);
euler_gimbal_lock_out.print();
euler_gimbal_lock.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);
Dcmf dcm_lock2(euler_gimbal_lock2);
Eulerf euler_gimbal_lock_out2(dcm_lock2);
euler_gimbal_lock_out2.print();
euler_gimbal_lock2.print();
assert(euler_gimbal_lock2 == euler_gimbal_lock_out2);
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */