Merge branch 'master' into euler_fix

Conflicts:
	.travis.yml
	test/attitude.cpp

matrix/Matrix.hpp

@@ -13,6 +13,8 @@
 #include <stdlib.h>
 #include <string.h>
 #include <math.h>
+#include <cassert>
+#include <stdexcept>
 #if defined(SUPPORT_STDIOSTREAM)
 #include <iostream>
 #include <iomanip>
@@ -264,20 +266,37 @@ public:
      * Misc. Functions
      */
 
-    void print() const
+    void write_string(char * buf, size_t n) const
     {
         const Matrix<Type, M, N> &self = *this;
-        printf("\n");
-
+        char data_buf[500] = {0};
         for (size_t i = 0; i < M; i++) {
-            printf("[");
-
+            char data_line[100] = {0};
+            char data_line_formatted[100] = {0};
             for (size_t j = 0; j < N; j++) {
-                printf("%10g\t", double(self(i, j)));
+                char val_buf[15];
+                if (j == N-1) {
+                    snprintf(val_buf, 15, "\t%10g", double(self(i, j)));
+                } else {
+                    snprintf(val_buf, 15, "\t%10g,", double(self(i, j)));
+                }
+                strncat(data_line, val_buf, 300);
             }
-
-            printf("]\n");
+            if (i == M-1) {
+                snprintf(data_line_formatted, n, "[%s]", data_line);
+            } else {
+                snprintf(data_line_formatted, n, "[%s],\n", data_line);
+            }
+            strncat(data_buf, data_line_formatted, n);
         }
+        snprintf(buf, n, "[%s]", data_buf);
+    }
+
+    void print() const
+    {
+        char buf[200];
+        write_string(buf, 200);
+        printf("%s\n", buf);
     }
 
     Matrix<Type, N, M> transpose() const
@@ -294,7 +313,6 @@ public:
         return res;
     }
 
-
     // tranpose alias
     inline Matrix<Type, N, M> T() const
     {
@@ -423,6 +441,19 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
     return other * scalar;
 }
 
+template<typename Type, size_t  M, size_t N>
+bool isEqual(const Matrix<Type, M, N> &x,
+             const Matrix<Type, M, N> & y) {
+    if (!(x == y)) {
+        char buf_x[100];
+        char buf_y[100];
+        x.write_string(buf_x, 100);
+        y.write_string(buf_y, 100);
+        printf("not equal\nx:\n%s\ny:\n%s\n", buf_x, buf_y);
+    }
+    return x == y;
+}
+
 #if defined(SUPPORT_STDIOSTREAM)
 template<typename Type, size_t  M, size_t N>
 std::ostream& operator<<(std::ostream& os,

matrix/helper_functions.hpp

@@ -4,7 +4,7 @@
 
 // grody hack - this should go once C++11 is supported
 // on all platforms.
-#ifdef __PX4_NUTTX
+#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
 #include <math.h>
 #else
 #include <cmath>
@@ -16,7 +16,7 @@ namespace matrix
 template<typename Type>
 Type wrap_pi(Type x)
 {
-#ifdef __PX4_NUTTX
+#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
     if (!isfinite(x)) {
 #else
     if (!std::isfinite(x)) {

matrix/math.hpp

@@ -1,5 +1,8 @@
 #pragma once
 
+#ifdef __PX4_QURT
+#include "dspal_math.h"
+#endif
 #include "Matrix.hpp"
 #include "SquareMatrix.hpp"
 #include "Vector.hpp"