diff options
Diffstat (limited to 'src/lib/mathlib/math')
-rw-r--r-- | src/lib/mathlib/math/Limits.cpp | 5 | ||||
-rw-r--r-- | src/lib/mathlib/math/Limits.hpp | 4 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 107 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 2 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 10 |
5 files changed, 99 insertions, 29 deletions
diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a..e16f33bd6 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -46,6 +46,9 @@ namespace math { +#ifndef CONFIG_ARCH_ARM +#define M_PI_F 3.14159265358979323846f +#endif float __EXPORT min(float val1, float val2) { @@ -143,4 +146,4 @@ double __EXPORT degrees(double radians) return (radians / M_PI) * 180.0; } -}
\ No newline at end of file +} diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd66..fca4197b8 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -39,7 +39,7 @@ #pragma once -#include <nuttx/config.h> +#include <platforms/px4_defines.h> #include <stdint.h> namespace math { @@ -84,4 +84,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..806f5933a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,12 +44,20 @@ #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> +#define M_PI_2_F 1.5707963267948966192f +#endif namespace math { -template <unsigned int M, unsigned int N> +template<unsigned int M, unsigned int N> class __EXPORT Matrix; // MxN matrix with float elements @@ -65,16 +73,19 @@ 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 * Initializes the elements to zero. */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} - { + MatrixBase() : + data {}, + arm_mat {M, N, &data[0][0]} { } virtual ~MatrixBase() {}; @@ -83,20 +94,17 @@ public: * copyt ctor */ MatrixBase(const MatrixBase<M, N> &m) : - arm_mat{M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, m.data, sizeof(data)); } MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} - { + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} - { + MatrixBase(const float d[M][N]) : + arm_mat {M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); } @@ -148,8 +156,9 @@ public: bool operator ==(const Matrix<M, N> &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) + if (data[i][j] != m.data[i][j]) { return false; + } return true; } @@ -160,8 +169,9 @@ public: bool operator !=(const Matrix<M, N> &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) + if (data[i][j] != m.data[i][j]) { return true; + } return false; } @@ -181,8 +191,9 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { res.data[i][j] = -data[i][j]; + } return res; } @@ -194,16 +205,18 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { res.data[i][j] = data[i][j] + m.data[i][j]; + } return res; } Matrix<M, N> &operator +=(const Matrix<M, N> &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { data[i][j] += m.data[i][j]; + } return *static_cast<Matrix<M, N>*>(this); } @@ -215,16 +228,18 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res.data[i][j] = data[i][j] - m.data[i][j]; + } return res; } Matrix<M, N> &operator -=(const Matrix<M, N> &m) { for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int j = 0; j < M; j++) { data[i][j] -= m.data[i][j]; + } return *static_cast<Matrix<M, N>*>(this); } @@ -236,16 +251,19 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res.data[i][j] = data[i][j] * num; + } return res; + } Matrix<M, N> &operator *=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { data[i][j] *= num; + } return *static_cast<Matrix<M, N>*>(this); } @@ -254,16 +272,18 @@ public: Matrix<M, N> res; for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { res[i][j] = data[i][j] / num; + } return res; } Matrix<M, N> &operator /=(const float num) { for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { data[i][j] /= num; + } return *static_cast<Matrix<M, N>*>(this); } @@ -273,27 +293,53 @@ public: */ template <unsigned int P> Matrix<M, P> operator *(const Matrix<N, P> &m) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, P> res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> > + (this->arm_mat.pData); + Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> > + (m.arm_mat.pData); + Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him; + Matrix<M, P> res(Product.data()); + return res; +#endif } /** * transpose the matrix */ Matrix<N, M> transposed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<N, M> res; arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> > + (this->arm_mat.pData); + Me.transposeInPlace(); + Matrix<N, M> res(Me.data()); + return res; +#endif } /** * invert the matrix */ Matrix<M, N> inversed(void) const { +#ifdef CONFIG_ARCH_ARM Matrix<M, N> res; arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; +#else + Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> > + (this->arm_mat.pData); + Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + Matrix<M, N> res(MyInverse.data()); + return res; +#endif } /** @@ -310,16 +356,18 @@ public: memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; - for (unsigned int i = 0; i < n; i++) + for (unsigned int i = 0; i < n; i++) { data[i][i] = 1; + } } void print(void) { for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) + for (unsigned int j = 0; j < N; j++) { printf("%.3f\t", data[i][j]); + } printf(" ]\n"); } @@ -352,8 +400,17 @@ 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..b3cca30c6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,7 +44,7 @@ #define QUATERNION_HPP #include <math.h> -#include "../CMSIS/Include/arm_math.h" + #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 0ddf77615..57b45e3ab 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -45,7 +45,12 @@ #include <stdio.h> #include <math.h> + +#ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" +#else +#include <math/eigen_math.h> +#endif namespace math { @@ -65,7 +70,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 |