aboutsummaryrefslogtreecommitdiff
path: root/src/lib
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib')
-rw-r--r--src/lib/geo/geo.h5
-rw-r--r--src/lib/mathlib/math/Limits.cpp5
-rw-r--r--src/lib/mathlib/math/Limits.hpp4
-rw-r--r--src/lib/mathlib/math/Matrix.hpp79
-rw-r--r--src/lib/mathlib/math/Quaternion.hpp2
-rw-r--r--src/lib/mathlib/math/Vector.hpp12
m---------src/lib/uavcan0
7 files changed, 86 insertions, 21 deletions
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 2311e0a7c..ca3587b93 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -44,10 +44,7 @@
*/
#pragma once
-
-#include "uORB/topics/fence.h"
-#include "uORB/topics/vehicle_global_position.h"
-
+#include <platforms/px4_defines.h>
__BEGIN_DECLS
#include "geo_lookup/geo_mag_declination.h"
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 ac1f1538f..68f0a777d 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 <platforms/ros/eigen_math.h>
+#include <Eigen/Eigen>
+#endif
+#include <platforms/px4_defines.h>
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));
}
@@ -114,6 +122,15 @@ public:
memcpy(data, d, sizeof(data));
}
+#if defined(__PX4_ROS)
+ /**
+ * set data from boost::array
+ */
+ void set(const boost::array<float, 9ul> d) {
+ set(static_cast<const float*>(d.data()));
+ }
+#endif
+
/**
* access by index
*/
@@ -148,8 +165,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;
}
@@ -273,27 +291,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
}
/**
@@ -352,8 +396,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..781c14d53 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,14 @@
#include <stdio.h>
#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
+#else
+#include <platforms/ros/eigen_math.h>
+#endif
+
+#include <platforms/px4_defines.h>
namespace math
{
@@ -65,7 +72,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
diff --git a/src/lib/uavcan b/src/lib/uavcan
-Subproject 1efd24427539fa332a15151143466ec760fa5ff
+Subproject 6a193648985adab9665e31a9460be04bc91d896
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-08 19:36:23 +0000