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

Fixes for nuttx.

Browse Source
This commit is contained in:
jgoppert
2015-11-08 12:08:19 -05:00
parent ace2751715
commit 5c573b075f
2 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+4 -6
View File
@@ -10,8 +10,6 @@
#include "math.hpp" #include "math.hpp"
#include <cmath>
namespace matrix namespace matrix
{ {
@@ -54,17 +52,17 @@ public:
Type theta_val = Type(asin(-dcm(2,0))); Type theta_val = Type(asin(-dcm(2,0)));
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2))); Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
// protection against gimbal lock // protection against NaN if dcm(0,0) or dcm(2,2) == 0
psi() = 0; psi() = 0;
theta() = 0; theta() = 0;
phi() = 0; phi() = 0;
if (std::isfinite(psi_val)) { if (psi() >= -M_PI_2 && psi() <= M_PI_2) {
psi() = psi_val; psi() = psi_val;
} }
if (std::isfinite(theta_val)) { if (theta() >= -M_PI_2 && theta() <= M_PI_2) {
theta() = theta_val; theta() = theta_val;
} }
if (std::isfinite(phi_val)) { if (phi() >= -M_PI_2 && phi() <= M_PI_2) {
phi() = phi_val; phi() = phi_val;
} }
} }
matrix/SquareMatrix.hpp
+3
View File
@@ -22,6 +22,9 @@ namespace matrix
template <typename Type, size_t M, size_t N> template <typename Type, size_t M, size_t N>
class Matrix; class Matrix;
template <typename Type, size_t M>
class Vector;
template<typename Type, size_t M> template<typename Type, size_t M>
class SquareMatrix : public Matrix<Type, M, M> class SquareMatrix : public Matrix<Type, M, M>
{ {