From 5c573b075f7bae9bf1bacd654b2bfb5db0bb0a70 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Sun, 8 Nov 2015 12:08:19 -0500
Subject: [PATCH] Fixes for nuttx.

---
 matrix/Euler.hpp        | 10 ++++------
 matrix/SquareMatrix.hpp |  3 +++
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 3fe125d62c..74e364940f 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -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;
         }
     }
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index ffbe1045af..8281f8e4a4 100644
--- a/matrix/SquareMatrix.hpp
+++ b/matrix/SquareMatrix.hpp
@@ -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>
 {