fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:13:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+15 -12
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;
}
}
matrix/Matrix.hpp
+2 -2
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++) {