fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/ROS_shared_lib_base_class' into dev_ros_rossharedlib

Browse Source 
Conflicts:
	src/modules/uORB/topics/vehicle_attitude.h
	src/platforms/px4_defines.h
This commit is contained in:
Thomas Gubler
2014-12-02 16:52:38 +01:00
parent 05a87a706a efe0938e99
commit ad499a5944
22 changed files with 1271 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Limits.cpp
+4 -1
View File
@@ -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;
}
}
}
src/lib/mathlib/math/Limits.hpp
+2 -2
View File
@@ -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);
}
}
src/lib/mathlib/math/Matrix.hpp
+82 -25
View File
@@ -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;
}
};
src/lib/mathlib/math/Quaternion.hpp
+1 -1
View File
@@ -44,7 +44,7 @@
#define QUATERNION_HPP
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
#include "Vector.hpp"
#include "Matrix.hpp"
src/lib/mathlib/math/Vector.hpp
+10
View File
@@ -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