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

clang-tidy fix mathlib headers and format

Browse Source
This commit is contained in:
Daniel Agar
2017-06-02 19:35:18 -04:00
parent ac4a706da0
commit e112161a04
14 changed files with 438 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/files_to_check_code_style.sh
-3
View File
@@ -9,13 +9,10 @@ then
fi fi
exec find src \ exec find src \
-path src/drivers/bootloaders -o \
-path src/examples/attitude_estimator_ekf -prune -o \ -path src/examples/attitude_estimator_ekf -prune -o \
-path src/examples/ekf_att_pos_estimator -prune -o \ -path src/examples/ekf_att_pos_estimator -prune -o \
-path src/lib/DriverFramework -prune -o \ -path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \ -path src/lib/ecl -prune -o \
-path src/lib/external_lgpl -prune -o \
-path src/lib/mathlib -prune -o \
-path src/lib/matrix -prune -o \ -path src/lib/matrix -prune -o \
-path src/modules/commander -prune -o \ -path src/modules/commander -prune -o \
-path src/modules/sdlog2 -prune -o \ -path src/modules/sdlog2 -prune -o \
src/examples/segway/BlockSegwayController.hpp
+1 -1
View File
@@ -11,7 +11,7 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot
{ {
public: public:
BlockSegwayController() : BlockSegwayController() :
BlockUorbEnabledAutopilot(NULL, "SEG"), BlockUorbEnabledAutopilot(nullptr, "SEG"),
th2v(this, "TH2V"), th2v(this, "TH2V"),
q2v(this, "Q2V"), q2v(this, "Q2V"),
_attPoll(), _attPoll(),
src/lib/DriverFramework
+1 -1
Submodule src/lib/DriverFramework updated: 87de5cbab3...a2ef69ec51
src/lib/controllib/block/Block.hpp
+3 -3
View File
@@ -114,19 +114,19 @@ public:
{ {
Block::updateParams(); Block::updateParams();
if (getChildren().getHead() != NULL) { updateChildParams(); } if (getChildren().getHead() != nullptr) { updateChildParams(); }
} }
virtual void updateSubscriptions() virtual void updateSubscriptions()
{ {
Block::updateSubscriptions(); Block::updateSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); } if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); }
} }
virtual void updatePublications() virtual void updatePublications()
{ {
Block::updatePublications(); Block::updatePublications();
if (getChildren().getHead() != NULL) { updateChildPublications(); } if (getChildren().getHead() != nullptr) { updateChildPublications(); }
} }
protected: protected:
// methods // methods
src/lib/controllib/blocks.hpp
+2 -2
View File
@@ -544,10 +544,10 @@ public:
S = V1 * V1 + V2 * V2; S = V1 * V1 + V2 * V2;
} while (S >= 1 || fabsf(S) < 1e-8f); } while (S >= 1 || fabsf(S) < 1e-8f);
X = V1 * float(sqrt(-2 * float(log(S)) / S)); X = V1 * float(sqrtf(-2 * float(logf(S)) / S));
} else { } else {
X = V2 * float(sqrt(-2 * float(log(S)) / S)); X = V2 * float(sqrtf(-2 * float(logf(S)) / S));
} }
phase = 1 - phase; phase = 1 - phase;
src/lib/mathlib/math/Expo.hpp
+8 -7
View File
@@ -48,24 +48,25 @@ namespace math
{ {
// Type-safe signum function // Type-safe signum function
template<typename T> int sign(T val) { template<typename T> int sign(T val)
{
return (T(0) < val) - (val < T(0)); return (T(0) < val) - (val < T(0));
} }
template<typename _Tp> template<typename _Tp>
inline const _Tp expo(const _Tp &value, const _Tp &e) inline const _Tp expo(const _Tp &value, const _Tp &e)
{ {
_Tp x = constrain(value ,(_Tp)-1, (_Tp)1); _Tp x = constrain(value, (_Tp) - 1, (_Tp)1);
return (1-e)*x + e*x*x*x; return (1 - e) * x + e * x * x * x;
} }
template<typename _Tp> template<typename _Tp>
inline const _Tp deadzone(const _Tp &value, const _Tp &dz) inline const _Tp deadzone(const _Tp &value, const _Tp &dz)
{ {
_Tp x = constrain(value ,(_Tp)-1, (_Tp)1); _Tp x = constrain(value, (_Tp) - 1, (_Tp)1);
_Tp dzc = constrain(dz ,(_Tp)-1, (_Tp)1); _Tp dzc = constrain(dz, (_Tp) - 1, (_Tp)1);
// Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone // Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone
_Tp out = (x-sign(x)*dzc)/(1-dzc); _Tp out = (x - sign(x) * dzc) / (1 - dzc);
// apply the deadzone (values zero around the middle) // apply the deadzone (values zero around the middle)
return out * (fabsf(x) > dzc); return out * (fabsf(x) > dzc);
} }
@@ -73,7 +74,7 @@ inline const _Tp deadzone(const _Tp &value, const _Tp &dz)
template<typename _Tp> template<typename _Tp>
inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz) inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
{ {
return expo(deadzone(value, dz),e); return expo(deadzone(value, dz), e);
} }
} }
src/lib/mathlib/math/Matrix.hpp
+123 -63
View File
@@ -76,7 +76,7 @@ public:
* trivial ctor * trivial ctor
* Initializes the elements to zero. * Initializes the elements to zero.
*/ */
MatrixBase() : MatrixBase() :
data{}, data{},
arm_mat{M, N, &data[0][0]} arm_mat{M, N, &data[0][0]}
{ {
@@ -99,7 +99,7 @@ public:
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
} }
MatrixBase(const float d[M][N]) : MatrixBase(const float d[M][N]) :
arm_mat{M, N, &data[0][0]} arm_mat{M, N, &data[0][0]}
{ {
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
@@ -108,21 +108,24 @@ public:
/** /**
* set data * set data
*/ */
void set(const float *d) { void set(const float *d)
{
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
} }
/** /**
* set data * set data
*/ */
void set(const float d[M][N]) { void set(const float d[M][N])
{
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
} }
/** /**
* set row from vector * set row from vector
*/ */
void set_row(unsigned int row, const Vector<N> v) { void set_row(unsigned int row, const Vector<N> v)
{
for (unsigned i = 0; i < N; i++) { for (unsigned i = 0; i < N; i++) {
data[row][i] = v.data[i]; data[row][i] = v.data[i];
} }
@@ -131,7 +134,8 @@ public:
/** /**
* set column from vector * set column from vector
*/ */
void set_col(unsigned int col, const Vector<M> v) { void set_col(unsigned int col, const Vector<M> v)
{
for (unsigned i = 0; i < M; i++) { for (unsigned i = 0; i < M; i++) {
data[i][col] = v.data[i]; data[i][col] = v.data[i];
} }
@@ -140,39 +144,47 @@ public:
/** /**
* access by index * access by index
*/ */
float &operator()(const unsigned int row, const unsigned int col) { float &operator()(const unsigned int row, const unsigned int col)
{
return data[row][col]; return data[row][col];
} }
/** /**
* access by index * access by index
*/ */
float operator()(const unsigned int row, const unsigned int col) const { float operator()(const unsigned int row, const unsigned int col) const
{
return data[row][col]; return data[row][col];
} }
/** /**
* get rows number * get rows number
*/ */
unsigned int get_rows() const { unsigned int get_rows() const
{
return M; return M;
} }
/** /**
* get columns number * get columns number
*/ */
unsigned int get_cols() const { unsigned int get_cols() const
{
return N; return N;
} }
/** /**
* test for equality * test for equality
*/ */
bool operator ==(const Matrix<M, N> &m) const { bool operator ==(const Matrix<M, N> &m) const
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++) {
if (data[i][j] != m.data[i][j]) for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return false; return false;
}
}
}
return true; return true;
} }
@@ -180,11 +192,15 @@ public:
/** /**
* test for inequality * test for inequality
*/ */
bool operator !=(const Matrix<M, N> &m) const { bool operator !=(const Matrix<M, N> &m) const
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++) {
if (data[i][j] != m.data[i][j]) for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return true; return true;
}
}
}
return false; return false;
} }
@@ -192,7 +208,8 @@ public:
/** /**
* set to value * set to value
*/ */
const Matrix<M, N> &operator =(const Matrix<M, N> &m) { const Matrix<M, N> &operator =(const Matrix<M, N> &m)
{
memcpy(data, m.data, sizeof(data)); memcpy(data, m.data, sizeof(data));
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -200,12 +217,15 @@ public:
/** /**
* negation * negation
*/ */
Matrix<M, N> operator -(void) const { Matrix<M, N> operator -(void) const
{
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) 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]; res.data[i][j] = -data[i][j];
}
}
return res; return res;
} }
@@ -213,20 +233,26 @@ public:
/** /**
* addition * addition
*/ */
Matrix<M, N> operator +(const Matrix<M, N> &m) const { Matrix<M, N> operator +(const Matrix<M, N> &m) const
{
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) 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]; res.data[i][j] = data[i][j] + m.data[i][j];
}
}
return res; return res;
} }
Matrix<M, N> &operator +=(const Matrix<M, N> &m) { Matrix<M, N> &operator +=(const Matrix<M, N> &m)
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] += m.data[i][j]; data[i][j] += m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -234,20 +260,26 @@ public:
/** /**
* subtraction * subtraction
*/ */
Matrix<M, N> operator -(const Matrix<M, N> &m) const { Matrix<M, N> operator -(const Matrix<M, N> &m) const
{
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) 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]; res.data[i][j] = data[i][j] - m.data[i][j];
}
}
return res; return res;
} }
Matrix<M, N> &operator -=(const Matrix<M, N> &m) { Matrix<M, N> &operator -=(const Matrix<M, N> &m)
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] -= m.data[i][j]; data[i][j] -= m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -255,38 +287,50 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
Matrix<M, N> operator *(const float num) const { Matrix<M, N> operator *(const float num) const
{
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) 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; res.data[i][j] = data[i][j] * num;
}
}
return res; return res;
} }
Matrix<M, N> &operator *=(const float num) { 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 i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] *= num; data[i][j] *= num;
}
}
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
Matrix<M, N> operator /(const float num) const { Matrix<M, N> operator /(const float num) const
{
Matrix<M, N> res; Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) 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; res.data[i][j] = data[i][j] / num;
}
}
return res; return res;
} }
Matrix<M, N> &operator /=(const float num) { 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 i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] /= num; data[i][j] /= num;
}
}
return *static_cast<Matrix<M, N>*>(this); return *static_cast<Matrix<M, N>*>(this);
} }
@@ -295,7 +339,8 @@ public:
* multiplication by another matrix * multiplication by another matrix
*/ */
template <unsigned int P> template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const { Matrix<M, P> operator *(const Matrix<N, P> &m) const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData); matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, N, P> Him(m.arm_mat.pData); matrix::Matrix<float, N, P> Him(m.arm_mat.pData);
matrix::Matrix<float, M, P> Product = Me * Him; matrix::Matrix<float, M, P> Product = Me * Him;
@@ -306,7 +351,8 @@ public:
/** /**
* transpose the matrix * transpose the matrix
*/ */
Matrix<N, M> transposed(void) const { Matrix<N, M> transposed(void) const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData); matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
Matrix<N, M> res(Me.transpose().data()); Matrix<N, M> res(Me.transpose().data());
return res; return res;
@@ -315,7 +361,8 @@ public:
/** /**
* invert the matrix * invert the matrix
*/ */
Matrix<M, N> inversed(void) const { Matrix<M, N> inversed(void) const
{
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData); matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
Matrix<M, N> res(Me.I().data()); Matrix<M, N> res(Me.I().data());
return res; return res;
@@ -324,27 +371,32 @@ public:
/** /**
* set zero matrix * set zero matrix
*/ */
void zero(void) { void zero(void)
{
memset(data, 0, sizeof(data)); memset(data, 0, sizeof(data));
} }
/** /**
* set identity matrix * set identity matrix
*/ */
void identity(void) { void identity(void)
{
memset(data, 0, sizeof(data)); memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N; 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; data[i][i] = 1;
}
} }
void print(void) { void print(void)
{
for (unsigned int i = 0; i < M; i++) { for (unsigned int i = 0; i < M; i++) {
printf("[ "); printf("[ ");
for (unsigned int j = 0; j < N; j++) for (unsigned int j = 0; j < N; j++) {
printf("%.3f\t", (double)data[i][j]); printf("%.3f\t", (double)data[i][j]);
}
printf(" ]\n"); printf(" ]\n");
} }
@@ -368,7 +420,8 @@ public:
/** /**
* set to value * set to value
*/ */
const Matrix<M, N> &operator =(const Matrix<M, N> &m) { const Matrix<M, N> &operator =(const Matrix<M, N> &m)
{
memcpy(this->data, m.data, sizeof(this->data)); memcpy(this->data, m.data, sizeof(this->data));
return *this; return *this;
} }
@@ -376,7 +429,8 @@ public:
/** /**
* multiplication by a vector * multiplication by a vector
*/ */
Vector<M> operator *(const Vector<N> &v) const { Vector<M> operator *(const Vector<N> &v) const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData); matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, N, 1> Vec(v.arm_col.pData); matrix::Matrix<float, N, 1> Vec(v.arm_col.pData);
matrix::Matrix<float, M, 1> Product = Me * Vec; matrix::Matrix<float, M, 1> Product = Me * Vec;
@@ -401,7 +455,8 @@ public:
/** /**
* set data * set data
*/ */
void set(const float d[9]) { void set(const float d[9])
{
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
} }
@@ -409,15 +464,17 @@ public:
/** /**
* set data from boost::array * set data from boost::array
*/ */
void set(const boost::array<float, 9ul> d) { void set(const boost::array<float, 9ul> d)
set(static_cast<const float*>(d.data())); {
set(static_cast<const float *>(d.data()));
} }
#endif #endif
/** /**
* set to value * set to value
*/ */
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { const Matrix<3, 3> &operator =(const Matrix<3, 3> &m)
{
memcpy(this->data, m.data, sizeof(this->data)); memcpy(this->data, m.data, sizeof(this->data));
return *this; return *this;
} }
@@ -425,7 +482,8 @@ public:
/** /**
* multiplication by a vector * multiplication by a vector
*/ */
Vector<3> operator *(const Vector<3> &v) const { Vector<3> operator *(const Vector<3> &v) const
{
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
@@ -436,7 +494,8 @@ public:
* create a rotation matrix from given euler angles * create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/ */
void from_euler(float roll, float pitch, float yaw) { void from_euler(float roll, float pitch, float yaw)
{
float cp = cosf(pitch); float cp = cosf(pitch);
float sp = sinf(pitch); float sp = sinf(pitch);
float sr = sinf(roll); float sr = sinf(roll);
@@ -458,7 +517,8 @@ public:
/** /**
* get euler angles from rotation matrix * get euler angles from rotation matrix
*/ */
Vector<3> to_euler(void) const { Vector<3> to_euler() const
{
Vector<3> euler; Vector<3> euler;
euler.data[1] = asinf(-data[2][0]); euler.data[1] = asinf(-data[2][0]);
src/lib/mathlib/math/Quaternion.hpp
+60 -43
View File
@@ -85,7 +85,8 @@ public:
/** /**
* multiplication * multiplication
*/ */
const Quaternion operator *(const Quaternion &q) const { const Quaternion operator *(const Quaternion &q) const
{
return Quaternion( return Quaternion(
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
@@ -96,20 +97,22 @@ public:
/** /**
* division * division
*/ */
Quaternion operator /(const Quaternion &q) const { Quaternion operator /(const Quaternion &q) const
{
float norm = q.length_squared(); float norm = q.length_squared();
return Quaternion( return Quaternion(
( data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm, (data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
(- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm, (- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
(- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm, (- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
(- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm (- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
); );
} }
/** /**
* derivative * derivative
*/ */
const Quaternion derivative(const Vector<3> &w) { const Quaternion derivative(const Vector<3> &w)
{
float dataQ[] = { float dataQ[] = {
data[0], -data[1], -data[2], -data[3], data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2], data[1], data[0], -data[3], data[2],
@@ -124,14 +127,16 @@ public:
/** /**
* conjugate * conjugate
*/ */
Quaternion conjugated() const { Quaternion conjugated() const
{
return Quaternion(data[0], -data[1], -data[2], -data[3]); return Quaternion(data[0], -data[1], -data[2], -data[3]);
} }
/** /**
* inversed * inversed
*/ */
Quaternion inversed() const { Quaternion inversed() const
{
float norm = length_squared(); float norm = length_squared();
return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm); return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
} }
@@ -139,62 +144,66 @@ public:
/** /**
* conjugation * conjugation
*/ */
Vector<3> conjugate(const Vector<3> &v) const { Vector<3> conjugate(const Vector<3> &v) const
{
float q0q0 = data[0] * data[0]; float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1]; float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2]; float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3]; float q3q3 = data[3] * data[3];
return Vector<3>( return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) + v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) + v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]), v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) + v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]), v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) + v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) + v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3) v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
); );
} }
/** /**
* conjugation with inversed quaternion * conjugation with inversed quaternion
*/ */
Vector<3> conjugate_inversed(const Vector<3> &v) const { Vector<3> conjugate_inversed(const Vector<3> &v) const
{
float q0q0 = data[0] * data[0]; float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1]; float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2]; float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3]; float q3q3 = data[3] * data[3];
return Vector<3>( return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) + v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) + v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]), v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) + v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]), v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) + v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) + v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3) v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
); );
} }
/** /**
* imaginary part of quaternion * imaginary part of quaternion
*/ */
Vector<3> imag(void) { Vector<3> imag()
{
return Vector<3>(&data[1]); return Vector<3>(&data[1]);
} }
/** /**
* set quaternion to rotation defined by euler angles * set quaternion to rotation defined by euler angles
*/ */
void from_euler(float roll, float pitch, float yaw) { void from_euler(float roll, float pitch, float yaw)
{
double cosPhi_2 = cos(double(roll) / 2.0); double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0); double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0); double cosTheta_2 = cos(double(pitch) / 2.0);
@@ -214,7 +223,8 @@ public:
/** /**
* simplified version of the above method to create quaternion representing rotation only by yaw * simplified version of the above method to create quaternion representing rotation only by yaw
*/ */
void from_yaw(float yaw) { void from_yaw(float yaw)
{
data[0] = cosf(yaw / 2.0f); data[0] = cosf(yaw / 2.0f);
data[1] = 0.0f; data[1] = 0.0f;
data[2] = 0.0f; data[2] = 0.0f;
@@ -224,20 +234,23 @@ public:
/** /**
* create Euler angles vector from the quaternion * create Euler angles vector from the quaternion
*/ */
Vector<3> to_euler() const { Vector<3> to_euler() const
{
return Vector<3>( return Vector<3>(
atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])), atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
asinf(2.0f * (data[0] * data[2] - data[3] * data[1])), asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3])) atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
); );
} }
/** /**
* set quaternion to rotation by DCM * set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
*/ */
void from_dcm(const Matrix<3, 3> &dcm) { void from_dcm(const Matrix<3, 3> &dcm)
{
float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]; float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f) { if (tr > 0.0f) {
float s = sqrtf(tr + 1.0f); float s = sqrtf(tr + 1.0f);
data[0] = s * 0.5f; data[0] = s * 0.5f;
@@ -245,19 +258,22 @@ public:
data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s; data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s; data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s; data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
} else { } else {
/* Find maximum diagonal element in dcm /* Find maximum diagonal element in dcm
* store index in dcm_i */ * store index in dcm_i */
int dcm_i = 0; int dcm_i = 0;
for (int i = 1; i < 3; i++) { for (int i = 1; i < 3; i++) {
if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) { if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
dcm_i = i; dcm_i = i;
} }
} }
int dcm_j = (dcm_i + 1) % 3; int dcm_j = (dcm_i + 1) % 3;
int dcm_k = (dcm_i + 2) % 3; int dcm_k = (dcm_i + 2) % 3;
float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] - float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k]) + 1.0f); dcm.data[dcm_k][dcm_k]) + 1.0f);
data[dcm_i + 1] = s * 0.5f; data[dcm_i + 1] = s * 0.5f;
s = 0.5f / s; s = 0.5f / s;
data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s; data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
@@ -269,7 +285,8 @@ public:
/** /**
* create rotation matrix for the quaternion * create rotation matrix for the quaternion
*/ */
Matrix<3, 3> to_dcm(void) const { Matrix<3, 3> to_dcm() const
{
Matrix<3, 3> R; Matrix<3, 3> R;
float aSq = data[0] * data[0]; float aSq = data[0] * data[0];
float bSq = data[1] * data[1]; float bSq = data[1] * data[1];
src/lib/mathlib/math/Vector.hpp
+149 -76
View File
@@ -106,38 +106,45 @@ public:
/** /**
* set data * set data
*/ */
void set(const float d[N]) { void set(const float d[N])
{
memcpy(data, d, sizeof(data)); memcpy(data, d, sizeof(data));
} }
/** /**
* access to elements by index * access to elements by index
*/ */
float &operator()(const unsigned int i) { float &operator()(const unsigned int i)
{
return data[i]; return data[i];
} }
/** /**
* access to elements by index * access to elements by index
*/ */
float operator()(const unsigned int i) const { float operator()(const unsigned int i) const
{
return data[i]; return data[i];
} }
/** /**
* get vector size * get vector size
*/ */
unsigned int get_size() const { unsigned int get_size() const
{
return N; return N;
} }
/** /**
* test for equality * test for equality
*/ */
bool operator ==(const Vector<N> &v) const { bool operator ==(const Vector<N> &v) const
for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) {
return false; return false;
}
}
return true; return true;
} }
@@ -145,10 +152,13 @@ public:
/** /**
* test for inequality * test for inequality
*/ */
bool operator !=(const Vector<N> &v) const { bool operator !=(const Vector<N> &v) const
for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) {
return true; return true;
}
}
return false; return false;
} }
@@ -156,7 +166,8 @@ public:
/** /**
* set to value * set to value
*/ */
const Vector<N> &operator =(const Vector<N> &v) { const Vector<N> &operator =(const Vector<N> &v)
{
memcpy(data, v.data, sizeof(data)); memcpy(data, v.data, sizeof(data));
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -164,11 +175,13 @@ public:
/** /**
* negation * negation
*/ */
const Vector<N> operator -(void) const { const Vector<N> operator -(void) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = -data[i]; res.data[i] = -data[i];
}
return res; return res;
} }
@@ -176,11 +189,13 @@ public:
/** /**
* addition * addition
*/ */
const Vector<N> operator +(const Vector<N> &v) const { const Vector<N> operator +(const Vector<N> &v) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] + v.data[i]; res.data[i] = data[i] + v.data[i];
}
return res; return res;
} }
@@ -188,11 +203,13 @@ public:
/** /**
* subtraction * subtraction
*/ */
const Vector<N> operator -(const Vector<N> &v) const { const Vector<N> operator -(const Vector<N> &v) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] - v.data[i]; res.data[i] = data[i] - v.data[i];
}
return res; return res;
} }
@@ -200,11 +217,13 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> operator *(const float num) const { const Vector<N> operator *(const float num) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] * num; res.data[i] = data[i] * num;
}
return res; return res;
} }
@@ -212,11 +231,13 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> operator /(const float num) const { const Vector<N> operator /(const float num) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] / num; res.data[i] = data[i] / num;
}
return res; return res;
} }
@@ -224,9 +245,11 @@ public:
/** /**
* addition * addition
*/ */
const Vector<N> &operator +=(const Vector<N> &v) { const Vector<N> &operator +=(const Vector<N> &v)
for (unsigned int i = 0; i < N; i++) {
for (unsigned int i = 0; i < N; i++) {
data[i] += v.data[i]; data[i] += v.data[i];
}
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -234,9 +257,11 @@ public:
/** /**
* subtraction * subtraction
*/ */
const Vector<N> &operator -=(const Vector<N> &v) { const Vector<N> &operator -=(const Vector<N> &v)
for (unsigned int i = 0; i < N; i++) {
for (unsigned int i = 0; i < N; i++) {
data[i] -= v.data[i]; data[i] -= v.data[i];
}
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -244,9 +269,11 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator *=(const float num) { const Vector<N> &operator *=(const float num)
for (unsigned int i = 0; i < N; i++) {
for (unsigned int i = 0; i < N; i++) {
data[i] *= num; data[i] *= num;
}
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -254,9 +281,11 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator /=(const float num) { const Vector<N> &operator /=(const float num)
for (unsigned int i = 0; i < N; i++) {
for (unsigned int i = 0; i < N; i++) {
data[i] /= num; data[i] /= num;
}
return *static_cast<const Vector<N>*>(this); return *static_cast<const Vector<N>*>(this);
} }
@@ -264,11 +293,13 @@ public:
/** /**
* dot product * dot product
*/ */
float operator *(const Vector<N> &v) const { float operator *(const Vector<N> &v) const
{
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res += data[i] * v.data[i]; res += data[i] * v.data[i];
}
return res; return res;
} }
@@ -276,11 +307,13 @@ public:
/** /**
* element by element multiplication * element by element multiplication
*/ */
const Vector<N> emult(const Vector<N> &v) const { const Vector<N> emult(const Vector<N> &v) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] * v.data[i]; res.data[i] = data[i] * v.data[i];
}
return res; return res;
} }
@@ -288,11 +321,13 @@ public:
/** /**
* element by element division * element by element division
*/ */
const Vector<N> edivide(const Vector<N> &v) const { const Vector<N> edivide(const Vector<N> &v) const
{
Vector<N> res; Vector<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] / v.data[i]; res.data[i] = data[i] / v.data[i];
}
return res; return res;
} }
@@ -300,11 +335,13 @@ public:
/** /**
* gets the length of this vector squared * gets the length of this vector squared
*/ */
float length_squared() const { float length_squared() const
{
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res += data[i] * data[i]; res += data[i] * data[i];
}
return res; return res;
} }
@@ -312,11 +349,13 @@ public:
/** /**
* gets the length of this vector * gets the length of this vector
*/ */
float length() const { float length() const
{
float res = 0.0f; float res = 0.0f;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
res += data[i] * data[i]; res += data[i] * data[i];
}
return sqrtf(res); return sqrtf(res);
} }
@@ -324,29 +363,34 @@ public:
/** /**
* normalizes this vector * normalizes this vector
*/ */
void normalize() { void normalize()
{
*this /= length(); *this /= length();
} }
/** /**
* returns the normalized version of this vector * returns the normalized version of this vector
*/ */
Vector<N> normalized() const { Vector<N> normalized() const
{
return *this / length(); return *this / length();
} }
/** /**
* set zero vector * set zero vector
*/ */
void zero(void) { void zero()
{
memset(data, 0, sizeof(data)); memset(data, 0, sizeof(data));
} }
void print(void) { void print(void)
{
printf("[ "); printf("[ ");
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++) {
printf("%.3f\t", (double)data[i]); printf("%.3f\t", (double)data[i]);
}
printf("]\n"); printf("]\n");
} }
@@ -365,7 +409,8 @@ public:
/** /**
* set to value * set to value
*/ */
const Vector<N> &operator =(const Vector<N> &v) { const Vector<N> &operator =(const Vector<N> &v)
{
memcpy(this->data, v.data, sizeof(this->data)); memcpy(this->data, v.data, sizeof(this->data));
return *this; return *this;
} }
@@ -378,17 +423,20 @@ public:
Vector() : VectorBase<2>() {} Vector() : VectorBase<2>() {}
// simple copy is 1.6 times faster than memcpy // simple copy is 1.6 times faster than memcpy
Vector(const Vector<2> &v) : VectorBase<2>() { Vector(const Vector<2> &v) : VectorBase<2>()
{
data[0] = v.data[0]; data[0] = v.data[0];
data[1] = v.data[1]; data[1] = v.data[1];
} }
Vector(const float d[2]) : VectorBase<2>() { Vector(const float d[2]) : VectorBase<2>()
{
data[0] = d[0]; data[0] = d[0];
data[1] = d[1]; data[1] = d[1];
} }
Vector(const float x, const float y) : VectorBase<2>() { Vector(const float x, const float y) : VectorBase<2>()
{
data[0] = x; data[0] = x;
data[1] = y; data[1] = y;
} }
@@ -396,7 +444,8 @@ public:
/** /**
* set data * set data
*/ */
void set(const float d[2]) { void set(const float d[2])
{
data[0] = d[0]; data[0] = d[0];
data[1] = d[1]; data[1] = d[1];
} }
@@ -404,20 +453,23 @@ public:
/** /**
* set data from boost::array * set data from boost::array
*/ */
void set(const boost::array<float, 2ul> d) { void set(const boost::array<float, 2ul> d)
set(static_cast<const float*>(d.data())); {
set(static_cast<const float *>(d.data()));
} }
#endif #endif
/** /**
* set to value * set to value
*/ */
const Vector<2> &operator =(const Vector<2> &v) { const Vector<2> &operator =(const Vector<2> &v)
{
data[0] = v.data[0]; data[0] = v.data[0];
data[1] = v.data[1]; data[1] = v.data[1];
return *this; return *this;
} }
float operator %(const Vector<2> &v) const { float operator %(const Vector<2> &v) const
{
return data[0] * v.data[1] - data[1] * v.data[0]; return data[0] * v.data[1] - data[1] * v.data[0];
} }
}; };
@@ -429,17 +481,22 @@ public:
Vector() : VectorBase<3>() {} Vector() : VectorBase<3>() {}
// simple copy is 1.6 times faster than memcpy // simple copy is 1.6 times faster than memcpy
Vector(const Vector<3> &v) : VectorBase<3>() { Vector(const Vector<3> &v) : VectorBase<3>()
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int i = 0; i < 3; i++) {
data[i] = v.data[i]; data[i] = v.data[i];
}
} }
Vector(const float d[3]) : VectorBase<3>() { Vector(const float d[3]) : VectorBase<3>()
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int i = 0; i < 3; i++) {
data[i] = d[i]; data[i] = d[i];
}
} }
Vector(const float x, const float y, const float z) : VectorBase<3>() { Vector(const float x, const float y, const float z) : VectorBase<3>()
{
data[0] = x; data[0] = x;
data[1] = y; data[1] = y;
data[2] = z; data[2] = z;
@@ -448,30 +505,36 @@ public:
/** /**
* set data from boost::array * set data from boost::array
*/ */
void set(const boost::array<float, 3ul> d) { void set(const boost::array<float, 3ul> d)
set(static_cast<const float*>(d.data())); {
set(static_cast<const float *>(d.data()));
} }
#endif #endif
/** /**
* set data * set data
*/ */
void set(const float d[3]) { void set(const float d[3])
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int i = 0; i < 3; i++) {
data[i] = d[i]; data[i] = d[i];
}
} }
/** /**
* set to value * set to value
*/ */
const Vector<3> &operator =(const Vector<3> &v) { const Vector<3> &operator =(const Vector<3> &v)
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int i = 0; i < 3; i++) {
data[i] = v.data[i]; data[i] = v.data[i];
}
return *this; return *this;
} }
Vector<3> operator %(const Vector<3> &v) const { Vector<3> operator %(const Vector<3> &v) const
{
return Vector<3>( return Vector<3>(
data[1] * v.data[2] - data[2] * v.data[1], data[1] * v.data[2] - data[2] * v.data[1],
data[2] * v.data[0] - data[0] * v.data[2], data[2] * v.data[0] - data[0] * v.data[2],
@@ -486,17 +549,22 @@ class __EXPORT Vector<4> : public VectorBase<4>
public: public:
Vector() : VectorBase() {} Vector() : VectorBase() {}
Vector(const Vector<4> &v) : VectorBase<4>() { Vector(const Vector<4> &v) : VectorBase<4>()
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int i = 0; i < 4; i++) {
data[i] = v.data[i]; data[i] = v.data[i];
}
} }
Vector(const float d[4]) : VectorBase<4>() { Vector(const float d[4]) : VectorBase<4>()
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int i = 0; i < 4; i++) {
data[i] = d[i]; data[i] = d[i];
}
} }
Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase()
{
data[0] = x0; data[0] = x0;
data[1] = x1; data[1] = x1;
data[2] = x2; data[2] = x2;
@@ -506,25 +574,30 @@ public:
/** /**
* set data from boost::array * set data from boost::array
*/ */
void set(const boost::array<float, 4ul> d) { void set(const boost::array<float, 4ul> d)
set(static_cast<const float*>(d.data())); {
set(static_cast<const float *>(d.data()));
} }
#endif #endif
/** /**
* set data * set data
*/ */
void set(const float d[4]) { void set(const float d[4])
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int i = 0; i < 4; i++) {
data[i] = d[i]; data[i] = d[i];
}
} }
/** /**
* set to value * set to value
*/ */
const Vector<4> &operator =(const Vector<4> &v) { const Vector<4> &operator =(const Vector<4> &v)
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int i = 0; i < 4; i++) {
data[i] = v.data[i]; data[i] = v.data[i];
}
return *this; return *this;
} }
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+39 -34
View File
@@ -34,7 +34,7 @@
****************************************************************************/ ****************************************************************************/
/// @file LowPassFilter.cpp /// @file LowPassFilter.cpp
/// @brief A class to implement a second order low pass filter /// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com> /// Author: Leonard Hall <LeonardTHall@gmail.com>
#include <px4_defines.h> #include <px4_defines.h>
@@ -46,48 +46,53 @@ namespace math
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{ {
_cutoff_freq = cutoff_freq; _cutoff_freq = cutoff_freq;
if (_cutoff_freq <= 0.0f) {
// no filtering if (_cutoff_freq <= 0.0f) {
return; // no filtering
} return;
float fr = sample_freq/_cutoff_freq; }
float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; float fr = sample_freq / _cutoff_freq;
_b0 = ohm*ohm/c; float ohm = tanf(M_PI_F / fr);
_b1 = 2.0f*_b0; float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
_b2 = _b0; _b0 = ohm * ohm / c;
_a1 = 2.0f*(ohm*ohm-1.0f)/c; _b1 = 2.0f * _b0;
_a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; _b2 = _b0;
_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
} }
float LowPassFilter2p::apply(float sample) float LowPassFilter2p::apply(float sample)
{ {
if (_cutoff_freq <= 0.0f) { if (_cutoff_freq <= 0.0f) {
// no filtering // no filtering
return sample; return sample;
} }
// do the filtering // do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (!PX4_ISFINITE(delay_element_0)) {
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
// return the value. Should be no need to check limits if (!PX4_ISFINITE(delay_element_0)) {
return output; // don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
// return the value. Should be no need to check limits
return output;
} }
float LowPassFilter2p::reset(float sample) { float LowPassFilter2p::reset(float sample)
{
float dval = sample / (_b0 + _b1 + _b2); float dval = sample / (_b0 + _b1 + _b2);
_delay_element_1 = dval; _delay_element_1 = dval;
_delay_element_2 = dval; _delay_element_2 = dval;
return apply(sample); return apply(sample);
} }
} // namespace math } // namespace math
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+44 -43
View File
@@ -34,7 +34,7 @@
****************************************************************************/ ****************************************************************************/
/// @file LowPassFilter.h /// @file LowPassFilter.h
/// @brief A class to implement a second order low pass filter /// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com> /// Author: Leonard Hall <LeonardTHall@gmail.com>
/// Adapted for PX4 by Andrew Tridgell /// Adapted for PX4 by Andrew Tridgell
@@ -45,54 +45,55 @@ namespace math
class __EXPORT LowPassFilter2p class __EXPORT LowPassFilter2p
{ {
public: public:
// constructor // constructor
LowPassFilter2p(float sample_freq, float cutoff_freq) : LowPassFilter2p(float sample_freq, float cutoff_freq) :
_cutoff_freq(cutoff_freq), _cutoff_freq(cutoff_freq),
_a1(0.0f), _a1(0.0f),
_a2(0.0f), _a2(0.0f),
_b0(0.0f), _b0(0.0f),
_b1(0.0f), _b1(0.0f),
_b2(0.0f), _b2(0.0f),
_delay_element_1(0.0f), _delay_element_1(0.0f),
_delay_element_2(0.0f) _delay_element_2(0.0f)
{ {
// set initial parameters // set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq); set_cutoff_frequency(sample_freq, cutoff_freq);
} }
/** /**
* Change filter parameters * Change filter parameters
*/ */
void set_cutoff_frequency(float sample_freq, float cutoff_freq); void set_cutoff_frequency(float sample_freq, float cutoff_freq);
/** /**
* Add a new raw value to the filter * Add a new raw value to the filter
* *
* @return retrieve the filtered result * @return retrieve the filtered result
*/ */
float apply(float sample); float apply(float sample);
/** /**
* Return the cutoff frequency * Return the cutoff frequency
*/ */
float get_cutoff_freq(void) const { float get_cutoff_freq() const
return _cutoff_freq; {
} return _cutoff_freq;
}
/** /**
* Reset the filter state to this value * Reset the filter state to this value
*/ */
float reset(float sample); float reset(float sample);
private: private:
float _cutoff_freq; float _cutoff_freq;
float _a1; float _a1;
float _a2; float _a2;
float _b0; float _b0;
float _b1; float _b1;
float _b2; float _b2;
float _delay_element_1; // buffered sample -1 float _delay_element_1; // buffered sample -1
float _delay_element_2; // buffered sample -2 float _delay_element_2; // buffered sample -2
}; };
} // namespace math } // namespace math
src/lib/mathlib/math/test/test.cpp
+5
View File
@@ -58,6 +58,7 @@ bool __EXPORT greater_than(float a, float b)
{ {
if (a > b) { if (a > b) {
return true; return true;
} else { } else {
printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
@@ -68,6 +69,7 @@ bool __EXPORT less_than(float a, float b)
{ {
if (a < b) { if (a < b) {
return true; return true;
} else { } else {
printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
@@ -78,6 +80,7 @@ bool __EXPORT greater_than_or_equal(float a, float b)
{ {
if (a >= b) { if (a >= b) {
return true; return true;
} else { } else {
printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
@@ -88,6 +91,7 @@ bool __EXPORT less_than_or_equal(float a, float b)
{ {
if (a <= b) { if (a <= b) {
return true; return true;
} else { } else {
printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
return false; return false;
@@ -122,6 +126,7 @@ void __EXPORT float2SigExp(
} else { } else {
exp = floor(exp); exp = floor(exp);
} }
#endif #endif
sig = num; sig = num;
src/modules/uORB/uORBManager.hpp
+1 -1
View File
@@ -364,7 +364,7 @@ public:
/** /**
* Gets the uORB Communicator instance. * Gets the uORB Communicator instance.
*/ */
uORBCommunicator::IChannel *get_uorb_communicator(void); uORBCommunicator::IChannel *get_uorb_communicator();
/** /**
* Utility method to check if there is a remote subscriber present * Utility method to check if there is a remote subscriber present
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+2 -2
View File
@@ -86,7 +86,7 @@ private:
// Disallow copy // Disallow copy
UnitTest(const uORBTest::UnitTest &) {}; UnitTest(const uORBTest::UnitTest &) {};
static int pubsubtest_threadEntry(char *const argv[]); static int pubsubtest_threadEntry(char *const argv[]);
int pubsublatency_main(void); int pubsublatency_main();
static int pub_test_multi2_entry(char *const argv[]); static int pub_test_multi2_entry(char *const argv[]);
int pub_test_multi2_main(); int pub_test_multi2_main();
@@ -133,7 +133,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
return test_fail("orb_advertise failed (%i)", errno); return test_fail("orb_advertise failed (%i)", errno);
} }
char *const args[1] = { NULL }; char *const args[1] = { nullptr };
pubsubtest_print = print; pubsubtest_print = print;
pubsubtest_passed = false; pubsubtest_passed = false;