diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-09-23 16:55:19 +0200 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-09-23 16:55:19 +0200 |
commit | 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 (patch) | |
tree | 3e0c39f7fce7b3f7697c7635c835f1fbd9c00a92 /src/lib | |
parent | 9ddb21404edc5a38eab47643f600e6114e8ece8e (diff) | |
download | px4-firmware-475f9a594b1088e8cb34b2d7c6f550de5a0193d7.tar.gz px4-firmware-475f9a594b1088e8cb34b2d7c6f550de5a0193d7.tar.bz2 px4-firmware-475f9a594b1088e8cb34b2d7c6f550de5a0193d7.zip |
Adapted math library for use of PX4 and ROS as shared library. First version, it works but some things might still be ugly
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/mathlib/math/Limits.hpp | 5 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 20 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 4 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 11 |
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 |