Fix unit test, create matrix isfinite.

PX4 · Feb 3, 2017 · 230e847 · 230e847
1 parent 7e3eff7
commit 230e847
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 7 deletions.
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
@@ -239,9 +239,9 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     }
 
     //check sanity of results
-    for (uint8_t i = 0; i < M; i++) {
-        for (uint8_t j = 0; j < M; j++) {
-            if (!PX4_ISFINITE(P(i,j))) {
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < M; j++) {
+            if (!isfinite(P(i,j))) {
                 return false;
             }
         }

diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
@@ -14,13 +14,18 @@ namespace matrix
 {
 
 template<typename Type>
-Type wrap_pi(Type x)
-{
+bool isfinite(Type x) {
 #if defined (__PX4_NUTTX) || defined (__PX4_QURT)
-    if (!isfinite(x)) {
+    return PX4_ISFINITE(x);
 #else
-    if (!std::isfinite(x)) {
+    return std::isfinite(x);
 #endif
+}
+
+template<typename Type>
+Type wrap_pi(Type x)
+{
+    if (!isfinite(x)) {
         return x;
     }