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

More testing and cleanup.

Browse Source
This commit is contained in:
jgoppert
2015-11-05 12:28:39 -05:00
parent 9d88b0d573
commit fd64e7e93a
10 changed files with 74 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dcm.hpp
+6 -4
View File
@@ -8,9 +8,8 @@
#pragma once
#include <Matrix.hpp>
#include <Quaternion.hpp>
#include <Euler.hpp>
#include "matrix.hpp"
namespace matrix
{
@@ -18,6 +17,9 @@ namespace matrix
template<typename Type>
class Quaternion;
template<typename Type>
class Euler;
template<typename Type>
class Dcm : public Matrix<Type, 3, 3>
{
@@ -28,7 +30,7 @@ public:
Dcm() : Matrix<Type, 3, 3>()
{
Matrix<Type, 3, 3>::setIdentity();
(*this) = eye<Type, 3>();
}
Dcm(const Type *data_) : Matrix<Type, 3, 3>(data_)
matrix/Euler.hpp
+15 -2
View File
@@ -30,6 +30,16 @@ public:
{
}
Euler(const Vector<Type, 3> & other) :
Vector<Type, 3>(other)
{
}
Euler(const Matrix<Type, 3, 1> & other) :
Vector<Type, 3>(other)
{
}
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
{
phi() = phi_;
@@ -37,7 +47,8 @@ public:
psi() = psi_;
}
Euler(const Dcm<Type> & dcm) {
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
theta() = Type(asin(-dcm(2, 0)));
if (fabs(theta() - M_PI_2) < 1.0e-3) {
@@ -54,7 +65,9 @@ public:
}
}
Euler(const Quaternion<Type> & q) {
Euler(const Quaternion<Type> & q) :
Vector<Type, 3>()
{
*this = Euler(Dcm<Type>(q));
}
matrix/Matrix.hpp
+4 -6
View File
@@ -26,11 +26,10 @@ template<typename Type, size_t M, size_t N>
class Matrix
{
protected:
Type _data[M][N];
public:
Type _data[M][N];
virtual ~Matrix() {};
Matrix() :
@@ -111,9 +110,8 @@ public:
bool operator==(const Matrix<Type, M, N> &other) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
static const Type eps = Type(1e-7);
static const Type eps = Type(1e-6);
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
@@ -336,7 +334,7 @@ public:
};
template<typename Type, size_t M, size_t N>
Matrix<Type, M, N> & zero() {
Matrix<Type, M, N> zero() {
Matrix<Type, M, N> m;
m.setZero();
return m;
matrix/Quaternion.hpp
+15 -2
View File
@@ -36,6 +36,16 @@ public:
q(3) = 0;
}
Quaternion(const Vector<Type, 4> & other) :
Vector<Type, 4>(other)
{
}
Quaternion(const Matrix<Type, 4, 1> & other) :
Vector<Type, 4>(other)
{
}
Quaternion(const Dcm<Type> & dcm) :
Vector<Type, 4>()
{
@@ -50,7 +60,9 @@ public:
(4 * q(0)));
}
Quaternion(const Euler<Type> & euler) {
Quaternion(const Euler<Type> & euler) :
Vector<Type, 4>()
{
Quaternion &q = *this;
Type cosPhi_2 = Type(cos(euler.phi() / 2.0));
Type cosTheta_2 = Type(cos(euler.theta() / 2.0));
@@ -68,7 +80,8 @@ public:
sinPhi_2 * sinTheta_2 * cosPsi_2;
}
Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
Quaternion(Type a, Type b, Type c, Type d) :
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = a;
matrix/Vector.hpp
-89
View File
@@ -64,95 +64,6 @@ public:
inline void normalize() {
(*this) /= norm();
}
/**
* Vector Operations
*/
Vector<Type, M> operator+(const Vector<Type, M> &other) const
{
Vector<Type, M> res;
const Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i) + other(i);
}
return res;
}
Vector<Type, M> operator-(const Vector<Type, M> &other) const
{
Vector<Type, M> res;
const Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i) - other(i);
}
return res;
}
void operator+=(const Vector<Type, M> &other)
{
Vector<Type, M> &self = *this;
self = self + other;
}
void operator-=(const Vector<Type, M> &other)
{
Vector<Type, M> &self = *this;
self = self - other;
}
/**
* Scalar Operations
*/
Vector<Type, M> operator*(Type scalar) const
{
Vector<Type, M> res;
const Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i) * scalar;
}
return res;
}
Vector<Type, M> operator/(Type scalar) const
{
return (*this)*(Type(1.0)/scalar);
}
Vector<Type, M> operator+(Type scalar) const
{
Vector<Type, M> res;
const Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i) + scalar;
}
return res;
}
void operator*=(Type scalar)
{
Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
self(i) = self(i) * scalar;
}
}
void operator/=(Type scalar)
{
Vector<Type, M> &self = *this;
self = self * (1.0f / scalar);
}
};
}; // namespace matrix
matrix/matrix.hpp
+1 -1
View File
@@ -1,9 +1,9 @@
#pragma once
#include "Matrix.hpp"
#include "SquareMatrix.hpp"
#include "Vector.hpp"
#include "Vector3.hpp"
#include "Euler.hpp"
#include "Dcm.hpp"
#include "Scalar.hpp"
#include "SquareMatrix.hpp"