Adapted math library for use of PX4 and ROS as shared library. First version, it works but some things might still be ugly

4 files changed, 39 insertions, 1 deletions

diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd66..713cb51b5 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,10 @@
 
 #pragma once
 
+#ifdef CONFIG_ARCH_ARM
 #include <nuttx/config.h>
+#endif
+
 #include <stdint.h>
 
 namespace math {
@@ -84,4 +87,4 @@ float __EXPORT degrees(float radians);
 
 double __EXPORT degrees(double radians);
 
-}
\ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ca931e2da..8fb3caa0d 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -44,7 +44,15 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#define M_PI_2_F 1.570769
+#endif
 
 namespace math
 {
@@ -65,7 +73,11 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+	#else
+	eigen_matrix_instance arm_mat;
+	#endif
 
 	/**
 	 * trivial ctor
@@ -352,8 +364,16 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+		#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+		#else
+		//probably nicer if this could go into a function like "eigen_mat_mult" or so
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me  = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::VectorXf Vec  = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
+		Eigen::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+		#endif
 		return res;
 	}
 };
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 21d05c7ef..d8acc4443 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,7 +44,11 @@
 #define QUATERNION_HPP
 
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#endif
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 0ddf77615..51a84d27e 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,13 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#endif
 
 namespace math
 {
@@ -65,7 +71,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor