fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
matrix/Euler.hpp
View File

@ -10,8 +10,6 @@
#include "math.hpp"
#include <cmath>
namespace matrix
{
@ -54,17 +52,17 @@ public:
Type theta_val = Type(asin(-dcm(2,0)));
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;
theta() = 0;
phi() = 0;
if (std::isfinite(psi_val)) {
if (psi() >= -M_PI_2 && psi() <= M_PI_2) {
psi() = psi_val;
}
if (std::isfinite(theta_val)) {
if (theta() >= -M_PI_2 && theta() <= M_PI_2) {
theta() = theta_val;
}
if (std::isfinite(phi_val)) {
if (phi() >= -M_PI_2 && phi() <= M_PI_2) {
phi() = phi_val;
}
}

3
matrix/SquareMatrix.hpp
View File

@ -22,6 +22,9 @@ namespace matrix
template <typename Type, size_t M, size_t N>
class Matrix;
template <typename Type, size_t M>
class Vector;
template<typename Type, size_t M>
class SquareMatrix : public Matrix<Type, M, M>
{