From e112161a04b6dfa36fd9fca7d85bceb07c598df3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 23 Apr 2017 16:21:16 -0400 Subject: [PATCH] clang-tidy fix mathlib headers and format --- Tools/files_to_check_code_style.sh | 3 - src/examples/segway/BlockSegwayController.hpp | 2 +- src/lib/DriverFramework | 2 +- src/lib/controllib/block/Block.hpp | 6 +- src/lib/controllib/blocks.hpp | 4 +- src/lib/mathlib/math/Expo.hpp | 15 +- src/lib/mathlib/math/Matrix.hpp | 186 ++++++++++----- src/lib/mathlib/math/Quaternion.hpp | 103 ++++---- src/lib/mathlib/math/Vector.hpp | 225 ++++++++++++------ .../mathlib/math/filter/LowPassFilter2p.cpp | 73 +++--- .../mathlib/math/filter/LowPassFilter2p.hpp | 87 +++---- src/lib/mathlib/math/test/test.cpp | 5 + src/modules/uORB/uORBManager.hpp | 2 +- .../uORB/uORB_tests/uORBTest_UnitTest.hpp | 4 +- 14 files changed, 438 insertions(+), 279 deletions(-) diff --git a/Tools/files_to_check_code_style.sh b/Tools/files_to_check_code_style.sh index e26d0a3f92..8a39c6d587 100755 --- a/Tools/files_to_check_code_style.sh +++ b/Tools/files_to_check_code_style.sh @@ -9,13 +9,10 @@ then fi exec find src \ - -path src/drivers/bootloaders -o \ -path src/examples/attitude_estimator_ekf -prune -o \ -path src/examples/ekf_att_pos_estimator -prune -o \ -path src/lib/DriverFramework -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/modules/commander -prune -o \ -path src/modules/sdlog2 -prune -o \ diff --git a/src/examples/segway/BlockSegwayController.hpp b/src/examples/segway/BlockSegwayController.hpp index 33da80f85b..f160bfb216 100644 --- a/src/examples/segway/BlockSegwayController.hpp +++ b/src/examples/segway/BlockSegwayController.hpp @@ -11,7 +11,7 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot { public: BlockSegwayController() : - BlockUorbEnabledAutopilot(NULL, "SEG"), + BlockUorbEnabledAutopilot(nullptr, "SEG"), th2v(this, "TH2V"), q2v(this, "Q2V"), _attPoll(), diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 87de5cbab3..a2ef69ec51 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 87de5cbab3e652b4dfdf2161f2440437b89ee399 +Subproject commit a2ef69ec51579947d76accec427ca8f74a7938f7 diff --git a/src/lib/controllib/block/Block.hpp b/src/lib/controllib/block/Block.hpp index 556ab41df2..e1b42ffb5c 100644 --- a/src/lib/controllib/block/Block.hpp +++ b/src/lib/controllib/block/Block.hpp @@ -114,19 +114,19 @@ public: { Block::updateParams(); - if (getChildren().getHead() != NULL) { updateChildParams(); } + if (getChildren().getHead() != nullptr) { updateChildParams(); } } virtual void updateSubscriptions() { Block::updateSubscriptions(); - if (getChildren().getHead() != NULL) { updateChildSubscriptions(); } + if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); } } virtual void updatePublications() { Block::updatePublications(); - if (getChildren().getHead() != NULL) { updateChildPublications(); } + if (getChildren().getHead() != nullptr) { updateChildPublications(); } } protected: // methods diff --git a/src/lib/controllib/blocks.hpp b/src/lib/controllib/blocks.hpp index b508b35cf0..4f87fbd519 100644 --- a/src/lib/controllib/blocks.hpp +++ b/src/lib/controllib/blocks.hpp @@ -544,10 +544,10 @@ public: S = V1 * V1 + V2 * V2; } 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 { - X = V2 * float(sqrt(-2 * float(log(S)) / S)); + X = V2 * float(sqrtf(-2 * float(logf(S)) / S)); } phase = 1 - phase; diff --git a/src/lib/mathlib/math/Expo.hpp b/src/lib/mathlib/math/Expo.hpp index 5af0ffca2e..ff9f3d9a82 100644 --- a/src/lib/mathlib/math/Expo.hpp +++ b/src/lib/mathlib/math/Expo.hpp @@ -48,24 +48,25 @@ namespace math { // Type-safe signum function -template int sign(T val) { +template int sign(T val) +{ return (T(0) < val) - (val < T(0)); } template inline const _Tp expo(const _Tp &value, const _Tp &e) { - _Tp x = constrain(value ,(_Tp)-1, (_Tp)1); - return (1-e)*x + e*x*x*x; + _Tp x = constrain(value, (_Tp) - 1, (_Tp)1); + return (1 - e) * x + e * x * x * x; } template inline const _Tp deadzone(const _Tp &value, const _Tp &dz) { - _Tp x = constrain(value ,(_Tp)-1, (_Tp)1); - _Tp dzc = constrain(dz ,(_Tp)-1, (_Tp)1); + _Tp x = constrain(value, (_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 - _Tp out = (x-sign(x)*dzc)/(1-dzc); + _Tp out = (x - sign(x) * dzc) / (1 - dzc); // apply the deadzone (values zero around the middle) return out * (fabsf(x) > dzc); } @@ -73,7 +74,7 @@ inline const _Tp deadzone(const _Tp &value, const _Tp &dz) template 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); } } diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index e50fef6b1b..f9cb5f08a1 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -76,7 +76,7 @@ public: * trivial ctor * Initializes the elements to zero. */ - MatrixBase() : + MatrixBase() : data{}, arm_mat{M, N, &data[0][0]} { @@ -99,7 +99,7 @@ public: memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) : + MatrixBase(const float d[M][N]) : arm_mat{M, N, &data[0][0]} { memcpy(data, d, sizeof(data)); @@ -108,21 +108,24 @@ public: /** * set data */ - void set(const float *d) { + void set(const float *d) + { memcpy(data, d, sizeof(data)); } /** * set data */ - void set(const float d[M][N]) { + void set(const float d[M][N]) + { memcpy(data, d, sizeof(data)); } /** * set row from vector */ - void set_row(unsigned int row, const Vector v) { + void set_row(unsigned int row, const Vector v) + { for (unsigned i = 0; i < N; i++) { data[row][i] = v.data[i]; } @@ -131,7 +134,8 @@ public: /** * set column from vector */ - void set_col(unsigned int col, const Vector v) { + void set_col(unsigned int col, const Vector v) + { for (unsigned i = 0; i < M; i++) { data[i][col] = v.data[i]; } @@ -140,39 +144,47 @@ public: /** * 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]; } /** * 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]; } /** * get rows number */ - unsigned int get_rows() const { + unsigned int get_rows() const + { return M; } /** * get columns number */ - unsigned int get_cols() const { + unsigned int get_cols() const + { return N; } /** * test for equality */ - bool operator ==(const Matrix &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]) + bool operator ==(const Matrix &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]) { return false; + } + } + } return true; } @@ -180,11 +192,15 @@ public: /** * test for inequality */ - bool operator !=(const Matrix &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]) + bool operator !=(const Matrix &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]) { return true; + } + } + } return false; } @@ -192,7 +208,8 @@ public: /** * set to value */ - const Matrix &operator =(const Matrix &m) { + const Matrix &operator =(const Matrix &m) + { memcpy(data, m.data, sizeof(data)); return *static_cast*>(this); } @@ -200,12 +217,15 @@ public: /** * negation */ - Matrix operator -(void) const { + Matrix operator -(void) const + { Matrix res; - 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++) { res.data[i][j] = -data[i][j]; + } + } return res; } @@ -213,20 +233,26 @@ public: /** * addition */ - Matrix operator +(const Matrix &m) const { + Matrix operator +(const Matrix &m) const + { Matrix res; - 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++) { res.data[i][j] = data[i][j] + m.data[i][j]; + } + } return res; } - Matrix &operator +=(const Matrix &m) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + Matrix &operator +=(const Matrix &m) + { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) { data[i][j] += m.data[i][j]; + } + } return *static_cast*>(this); } @@ -234,20 +260,26 @@ public: /** * subtraction */ - Matrix operator -(const Matrix &m) const { + Matrix operator -(const Matrix &m) const + { Matrix res; - 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++) { res.data[i][j] = data[i][j] - m.data[i][j]; + } + } return res; } - Matrix &operator -=(const Matrix &m) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + Matrix &operator -=(const Matrix &m) + { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) { data[i][j] -= m.data[i][j]; + } + } return *static_cast*>(this); } @@ -255,38 +287,50 @@ public: /** * uniform scaling */ - Matrix operator *(const float num) const { + Matrix operator *(const float num) const + { Matrix res; - 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++) { res.data[i][j] = data[i][j] * num; + } + } return res; } - Matrix &operator *=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + Matrix &operator *=(const float num) + { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) { data[i][j] *= num; + } + } return *static_cast*>(this); } - Matrix operator /(const float num) const { + Matrix operator /(const float num) const + { Matrix res; - 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++) { res.data[i][j] = data[i][j] / num; + } + } return res; } - Matrix &operator /=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) + Matrix &operator /=(const float num) + { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) { data[i][j] /= num; + } + } return *static_cast*>(this); } @@ -295,7 +339,8 @@ public: * multiplication by another matrix */ template - Matrix operator *(const Matrix &m) const { + Matrix operator *(const Matrix &m) const + { matrix::Matrix Me(this->arm_mat.pData); matrix::Matrix Him(m.arm_mat.pData); matrix::Matrix Product = Me * Him; @@ -306,7 +351,8 @@ public: /** * transpose the matrix */ - Matrix transposed(void) const { + Matrix transposed(void) const + { matrix::Matrix Me(this->arm_mat.pData); Matrix res(Me.transpose().data()); return res; @@ -315,7 +361,8 @@ public: /** * invert the matrix */ - Matrix inversed(void) const { + Matrix inversed(void) const + { matrix::SquareMatrix Me = matrix::Matrix(this->arm_mat.pData); Matrix res(Me.I().data()); return res; @@ -324,27 +371,32 @@ public: /** * set zero matrix */ - void zero(void) { + void zero(void) + { memset(data, 0, sizeof(data)); } /** * set identity matrix */ - void identity(void) { + void identity(void) + { 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) { + 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", (double)data[i][j]); + } printf(" ]\n"); } @@ -368,7 +420,8 @@ public: /** * set to value */ - const Matrix &operator =(const Matrix &m) { + const Matrix &operator =(const Matrix &m) + { memcpy(this->data, m.data, sizeof(this->data)); return *this; } @@ -376,7 +429,8 @@ public: /** * multiplication by a vector */ - Vector operator *(const Vector &v) const { + Vector operator *(const Vector &v) const + { matrix::Matrix Me(this->arm_mat.pData); matrix::Matrix Vec(v.arm_col.pData); matrix::Matrix Product = Me * Vec; @@ -401,7 +455,8 @@ public: /** * set data */ - void set(const float d[9]) { + void set(const float d[9]) + { memcpy(data, d, sizeof(data)); } @@ -409,15 +464,17 @@ public: /** * set data from boost::array */ - void set(const boost::array d) { - set(static_cast(d.data())); + void set(const boost::array d) + { + set(static_cast(d.data())); } #endif /** * 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)); return *this; } @@ -425,7 +482,8 @@ public: /** * 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], 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]); @@ -436,7 +494,8 @@ public: * create a rotation matrix from given euler angles * 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 sp = sinf(pitch); float sr = sinf(roll); @@ -458,7 +517,8 @@ public: /** * get euler angles from rotation matrix */ - Vector<3> to_euler(void) const { + Vector<3> to_euler() const + { Vector<3> euler; euler.data[1] = asinf(-data[2][0]); diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 4f5ccc623b..34e1ecab3e 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -85,7 +85,8 @@ public: /** * multiplication */ - const Quaternion operator *(const Quaternion &q) const { + const Quaternion operator *(const Quaternion &q) const + { 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[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], @@ -96,20 +97,22 @@ public: /** * division */ - Quaternion operator /(const Quaternion &q) const { + Quaternion operator /(const Quaternion &q) const + { float norm = q.length_squared(); 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[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[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / 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[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 + ); } /** * derivative */ - const Quaternion derivative(const Vector<3> &w) { + const Quaternion derivative(const Vector<3> &w) + { float dataQ[] = { data[0], -data[1], -data[2], -data[3], data[1], data[0], -data[3], data[2], @@ -124,14 +127,16 @@ public: /** * conjugate */ - Quaternion conjugated() const { + Quaternion conjugated() const + { return Quaternion(data[0], -data[1], -data[2], -data[3]); } /** * inversed */ - Quaternion inversed() const { + Quaternion inversed() const + { float norm = length_squared(); return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm); } @@ -139,62 +144,66 @@ public: /** * conjugation */ - Vector<3> conjugate(const Vector<3> &v) const { + Vector<3> conjugate(const Vector<3> &v) const + { float q0q0 = data[0] * data[0]; float q1q1 = data[1] * data[1]; float q2q2 = data[2] * data[2]; float q3q3 = data[3] * data[3]; return Vector<3>( - v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) + - 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[0] * (q0q0 + q1q1 - q2q2 - q3q3) + + 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[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) + - v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + - v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]), + v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) + + v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + + 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[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) + - v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3) - ); + 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[2] * (q0q0 - q1q1 - q2q2 + q3q3) + ); } /** * 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 q1q1 = data[1] * data[1]; float q2q2 = data[2] * data[2]; float q3q3 = data[3] * data[3]; return Vector<3>( - v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) + - 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[0] * (q0q0 + q1q1 - q2q2 - q3q3) + + 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[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) + - v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + - v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]), + v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) + + v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) + + 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[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) + - v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3) - ); + 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[2] * (q0q0 - q1q1 - q2q2 + q3q3) + ); } /** * imaginary part of quaternion */ - Vector<3> imag(void) { + Vector<3> imag() + { return Vector<3>(&data[1]); } /** * 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 sinPhi_2 = sin(double(roll) / 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 */ - void from_yaw(float yaw) { + void from_yaw(float yaw) + { data[0] = cosf(yaw / 2.0f); data[1] = 0.0f; data[2] = 0.0f; @@ -224,20 +234,23 @@ public: /** * create Euler angles vector from the quaternion */ - Vector<3> to_euler() const { + Vector<3> to_euler() const + { 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])), - 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[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])), + 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 * 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]; + if (tr > 0.0f) { float s = sqrtf(tr + 1.0f); data[0] = s * 0.5f; @@ -245,19 +258,22 @@ public: data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s; data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s; data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s; + } else { /* Find maximum diagonal element in dcm * store index in dcm_i */ int dcm_i = 0; + for (int i = 1; i < 3; i++) { if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) { dcm_i = i; } } + int dcm_j = (dcm_i + 1) % 3; int dcm_k = (dcm_i + 2) % 3; 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; s = 0.5f / 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 */ - Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> to_dcm() const + { Matrix<3, 3> R; float aSq = data[0] * data[0]; float bSq = data[1] * data[1]; diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 9748c5509a..80370acbf2 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -106,38 +106,45 @@ public: /** * set data */ - void set(const float d[N]) { + void set(const float d[N]) + { memcpy(data, d, sizeof(data)); } /** * access to elements by index */ - float &operator()(const unsigned int i) { + float &operator()(const unsigned int i) + { return data[i]; } /** * access to elements by index */ - float operator()(const unsigned int i) const { + float operator()(const unsigned int i) const + { return data[i]; } /** * get vector size */ - unsigned int get_size() const { + unsigned int get_size() const + { return N; } /** * test for equality */ - bool operator ==(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) + bool operator ==(const Vector &v) const + { + for (unsigned int i = 0; i < N; i++) { + if (data[i] != v.data[i]) { return false; + } + } return true; } @@ -145,10 +152,13 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) + bool operator !=(const Vector &v) const + { + for (unsigned int i = 0; i < N; i++) { + if (data[i] != v.data[i]) { return true; + } + } return false; } @@ -156,7 +166,8 @@ public: /** * set to value */ - const Vector &operator =(const Vector &v) { + const Vector &operator =(const Vector &v) + { memcpy(data, v.data, sizeof(data)); return *static_cast*>(this); } @@ -164,11 +175,13 @@ public: /** * negation */ - const Vector operator -(void) const { + const Vector operator -(void) const + { Vector res; - for (unsigned int i = 0; i < N; i++) + for (unsigned int i = 0; i < N; i++) { res.data[i] = -data[i]; + } return res; } @@ -176,11 +189,13 @@ public: /** * addition */ - const Vector operator +(const Vector &v) const { + const Vector operator +(const Vector &v) const + { Vector 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]; + } return res; } @@ -188,11 +203,13 @@ public: /** * subtraction */ - const Vector operator -(const Vector &v) const { + const Vector operator -(const Vector &v) const + { Vector 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]; + } return res; } @@ -200,11 +217,13 @@ public: /** * uniform scaling */ - const Vector operator *(const float num) const { + const Vector operator *(const float num) const + { Vector res; - for (unsigned int i = 0; i < N; i++) + for (unsigned int i = 0; i < N; i++) { res.data[i] = data[i] * num; + } return res; } @@ -212,11 +231,13 @@ public: /** * uniform scaling */ - const Vector operator /(const float num) const { + const Vector operator /(const float num) const + { Vector res; - for (unsigned int i = 0; i < N; i++) + for (unsigned int i = 0; i < N; i++) { res.data[i] = data[i] / num; + } return res; } @@ -224,9 +245,11 @@ public: /** * addition */ - const Vector &operator +=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) + const Vector &operator +=(const Vector &v) + { + for (unsigned int i = 0; i < N; i++) { data[i] += v.data[i]; + } return *static_cast*>(this); } @@ -234,9 +257,11 @@ public: /** * subtraction */ - const Vector &operator -=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) + const Vector &operator -=(const Vector &v) + { + for (unsigned int i = 0; i < N; i++) { data[i] -= v.data[i]; + } return *static_cast*>(this); } @@ -244,9 +269,11 @@ public: /** * uniform scaling */ - const Vector &operator *=(const float num) { - for (unsigned int i = 0; i < N; i++) + const Vector &operator *=(const float num) + { + for (unsigned int i = 0; i < N; i++) { data[i] *= num; + } return *static_cast*>(this); } @@ -254,9 +281,11 @@ public: /** * uniform scaling */ - const Vector &operator /=(const float num) { - for (unsigned int i = 0; i < N; i++) + const Vector &operator /=(const float num) + { + for (unsigned int i = 0; i < N; i++) { data[i] /= num; + } return *static_cast*>(this); } @@ -264,11 +293,13 @@ public: /** * dot product */ - float operator *(const Vector &v) const { + float operator *(const Vector &v) const + { 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]; + } return res; } @@ -276,11 +307,13 @@ public: /** * element by element multiplication */ - const Vector emult(const Vector &v) const { + const Vector emult(const Vector &v) const + { Vector 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]; + } return res; } @@ -288,11 +321,13 @@ public: /** * element by element division */ - const Vector edivide(const Vector &v) const { + const Vector edivide(const Vector &v) const + { Vector 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]; + } return res; } @@ -300,11 +335,13 @@ public: /** * gets the length of this vector squared */ - float length_squared() const { + float length_squared() const + { 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]; + } return res; } @@ -312,11 +349,13 @@ public: /** * gets the length of this vector */ - float length() const { + float length() const + { 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]; + } return sqrtf(res); } @@ -324,29 +363,34 @@ public: /** * normalizes this vector */ - void normalize() { + void normalize() + { *this /= length(); } /** * returns the normalized version of this vector */ - Vector normalized() const { + Vector normalized() const + { return *this / length(); } /** * set zero vector */ - void zero(void) { + void zero() + { memset(data, 0, sizeof(data)); } - void print(void) { + void print(void) + { printf("[ "); - for (unsigned int i = 0; i < N; i++) + for (unsigned int i = 0; i < N; i++) { printf("%.3f\t", (double)data[i]); + } printf("]\n"); } @@ -365,7 +409,8 @@ public: /** * set to value */ - const Vector &operator =(const Vector &v) { + const Vector &operator =(const Vector &v) + { memcpy(this->data, v.data, sizeof(this->data)); return *this; } @@ -378,17 +423,20 @@ public: Vector() : VectorBase<2>() {} // 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[1] = v.data[1]; } - Vector(const float d[2]) : VectorBase<2>() { + Vector(const float d[2]) : VectorBase<2>() + { data[0] = d[0]; 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[1] = y; } @@ -396,7 +444,8 @@ public: /** * set data */ - void set(const float d[2]) { + void set(const float d[2]) + { data[0] = d[0]; data[1] = d[1]; } @@ -404,20 +453,23 @@ public: /** * set data from boost::array */ - void set(const boost::array d) { - set(static_cast(d.data())); + void set(const boost::array d) + { + set(static_cast(d.data())); } #endif /** * 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[1] = v.data[1]; 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]; } }; @@ -429,17 +481,22 @@ public: Vector() : VectorBase<3>() {} // simple copy is 1.6 times faster than memcpy - Vector(const Vector<3> &v) : VectorBase<3>() { - for (unsigned int i = 0; i < 3; i++) + Vector(const Vector<3> &v) : VectorBase<3>() + { + for (unsigned int i = 0; i < 3; i++) { data[i] = v.data[i]; + } } - Vector(const float d[3]) : VectorBase<3>() { - for (unsigned int i = 0; i < 3; i++) + Vector(const float d[3]) : VectorBase<3>() + { + for (unsigned int i = 0; i < 3; 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[1] = y; data[2] = z; @@ -448,30 +505,36 @@ public: /** * set data from boost::array */ - void set(const boost::array d) { - set(static_cast(d.data())); + void set(const boost::array d) + { + set(static_cast(d.data())); } #endif /** * set data */ - void set(const float d[3]) { - for (unsigned int i = 0; i < 3; i++) + void set(const float d[3]) + { + for (unsigned int i = 0; i < 3; i++) { data[i] = d[i]; + } } /** * set to value */ - const Vector<3> &operator =(const Vector<3> &v) { - for (unsigned int i = 0; i < 3; i++) + const Vector<3> &operator =(const Vector<3> &v) + { + for (unsigned int i = 0; i < 3; i++) { data[i] = v.data[i]; + } return *this; } - Vector<3> operator %(const Vector<3> &v) const { + Vector<3> operator %(const Vector<3> &v) const + { return Vector<3>( data[1] * v.data[2] - data[2] * v.data[1], data[2] * v.data[0] - data[0] * v.data[2], @@ -486,17 +549,22 @@ class __EXPORT Vector<4> : public VectorBase<4> public: Vector() : VectorBase() {} - Vector(const Vector<4> &v) : VectorBase<4>() { - for (unsigned int i = 0; i < 4; i++) + Vector(const Vector<4> &v) : VectorBase<4>() + { + for (unsigned int i = 0; i < 4; i++) { data[i] = v.data[i]; + } } - Vector(const float d[4]) : VectorBase<4>() { - for (unsigned int i = 0; i < 4; i++) + Vector(const float d[4]) : VectorBase<4>() + { + for (unsigned int i = 0; i < 4; 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[1] = x1; data[2] = x2; @@ -506,25 +574,30 @@ public: /** * set data from boost::array */ - void set(const boost::array d) { - set(static_cast(d.data())); + void set(const boost::array d) + { + set(static_cast(d.data())); } #endif /** * set data */ - void set(const float d[4]) { - for (unsigned int i = 0; i < 4; i++) + void set(const float d[4]) + { + for (unsigned int i = 0; i < 4; i++) { data[i] = d[i]; + } } /** * set to value */ - const Vector<4> &operator =(const Vector<4> &v) { - for (unsigned int i = 0; i < 4; i++) + const Vector<4> &operator =(const Vector<4> &v) + { + for (unsigned int i = 0; i < 4; i++) { data[i] = v.data[i]; + } return *this; } diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 75a3fde4c9..0bf248340c 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /// @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 #include @@ -46,48 +46,53 @@ namespace math void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) { - _cutoff_freq = cutoff_freq; - if (_cutoff_freq <= 0.0f) { - // 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; - _b0 = ohm*ohm/c; - _b1 = 2.0f*_b0; - _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; + _cutoff_freq = cutoff_freq; + + if (_cutoff_freq <= 0.0f) { + // 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; + _b0 = ohm * ohm / c; + _b1 = 2.0f * _b0; + _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) { - if (_cutoff_freq <= 0.0f) { - // no filtering - return sample; - } + if (_cutoff_freq <= 0.0f) { + // no filtering + return sample; + } - // do the filtering - 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; + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - // return the value. Should be no need to check limits - return output; + 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 + return output; } -float LowPassFilter2p::reset(float sample) { +float LowPassFilter2p::reset(float sample) +{ float dval = sample / (_b0 + _b1 + _b2); - _delay_element_1 = dval; - _delay_element_2 = dval; - return apply(sample); + _delay_element_1 = dval; + _delay_element_2 = dval; + return apply(sample); } } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 4360651751..d4176bc5c3 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -34,7 +34,7 @@ ****************************************************************************/ /// @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 /// Adapted for PX4 by Andrew Tridgell @@ -45,54 +45,55 @@ namespace math class __EXPORT LowPassFilter2p { public: - // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) : - _cutoff_freq(cutoff_freq), - _a1(0.0f), - _a2(0.0f), - _b0(0.0f), - _b1(0.0f), - _b2(0.0f), - _delay_element_1(0.0f), - _delay_element_2(0.0f) - { - // set initial parameters - set_cutoff_frequency(sample_freq, cutoff_freq); - } + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) : + _cutoff_freq(cutoff_freq), + _a1(0.0f), + _a2(0.0f), + _b0(0.0f), + _b1(0.0f), + _b2(0.0f), + _delay_element_1(0.0f), + _delay_element_2(0.0f) + { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + } - /** - * Change filter parameters - */ - void set_cutoff_frequency(float sample_freq, float cutoff_freq); + /** + * Change filter parameters + */ + void set_cutoff_frequency(float sample_freq, float cutoff_freq); - /** - * Add a new raw value to the filter - * - * @return retrieve the filtered result - */ - float apply(float sample); + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + float apply(float sample); - /** - * Return the cutoff frequency - */ - float get_cutoff_freq(void) const { - return _cutoff_freq; - } + /** + * Return the cutoff frequency + */ + float get_cutoff_freq() const + { + return _cutoff_freq; + } - /** - * Reset the filter state to this value - */ - float reset(float sample); + /** + * Reset the filter state to this value + */ + float reset(float sample); private: - float _cutoff_freq; - float _a1; - float _a2; - float _b0; - float _b1; - float _b2; - float _delay_element_1; // buffered sample -1 - float _delay_element_2; // buffered sample -2 + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 }; } // namespace math diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 1588511931..a91bcf54df 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -58,6 +58,7 @@ bool __EXPORT greater_than(float a, float b) { if (a > b) { return true; + } else { printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; @@ -68,6 +69,7 @@ bool __EXPORT less_than(float a, float b) { if (a < b) { return true; + } else { printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; @@ -78,6 +80,7 @@ bool __EXPORT greater_than_or_equal(float a, float b) { if (a >= b) { return true; + } else { printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; @@ -88,6 +91,7 @@ bool __EXPORT less_than_or_equal(float a, float b) { if (a <= b) { return true; + } else { printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; @@ -122,6 +126,7 @@ void __EXPORT float2SigExp( } else { exp = floor(exp); } + #endif sig = num; diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 5ac0c6c4cb..b183e20dfc 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -364,7 +364,7 @@ public: /** * 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 diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index 429ac9a4d8..c574e2559d 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -86,7 +86,7 @@ private: // Disallow copy UnitTest(const uORBTest::UnitTest &) {}; static int pubsubtest_threadEntry(char *const argv[]); - int pubsublatency_main(void); + int pubsublatency_main(); static int pub_test_multi2_entry(char *const argv[]); 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); } - char *const args[1] = { NULL }; + char *const args[1] = { nullptr }; pubsubtest_print = print; pubsubtest_passed = false;