fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-04-23 16:21:16 -04:00
parent ac4a706da0
commit e112161a04
14 changed files with 438 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/files_to_check_code_style.sh
View File

@ -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 \

2
src/examples/segway/BlockSegwayController.hpp
View File

@ -11,7 +11,7 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot
{
public:
BlockSegwayController() :
BlockUorbEnabledAutopilot(NULL, "SEG"),
BlockUorbEnabledAutopilot(nullptr, "SEG"),
th2v(this, "TH2V"),
q2v(this, "Q2V"),
_attPoll(),

2
src/lib/DriverFramework

@ -1 +1 @@
Subproject commit 87de5cbab3e652b4dfdf2161f2440437b89ee399
Subproject commit a2ef69ec51579947d76accec427ca8f74a7938f7

6
src/lib/controllib/block/Block.hpp
View File

@ -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

4
src/lib/controllib/blocks.hpp
View File

@ -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;

15
src/lib/mathlib/math/Expo.hpp
View File

@ -48,24 +48,25 @@ namespace math
{
// 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));
}
template<typename _Tp>
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<typename _Tp>
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<typename _Tp>
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);
}
}

186
src/lib/mathlib/math/Matrix.hpp
View File

@ -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<N> v) {
void set_row(unsigned int row, const Vector<N> 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<M> v) {
void set_col(unsigned int col, const Vector<M> 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, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
bool operator ==(const Matrix<M, N> &m) const
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return false;
}
}
}
return true;
}
@ -180,11 +192,15 @@ public:
/**
* test for inequality
*/
bool operator !=(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
bool operator !=(const Matrix<M, N> &m) const
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return true;
}
}
}
return false;
}
@ -192,7 +208,8 @@ public:
/**
* 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));
return *static_cast<Matrix<M, N>*>(this);
}
@ -200,12 +217,15 @@ public:
/**
* negation
*/
Matrix<M, N> operator -(void) const {
Matrix<M, N> operator -(void) const
{
Matrix<M, N> 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<M, N> operator +(const Matrix<M, N> &m) const {
Matrix<M, N> operator +(const Matrix<M, N> &m) const
{
Matrix<M, N> 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<M, N> &operator +=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
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++) {
data[i][j] += m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this);
}
@ -234,20 +260,26 @@ public:
/**
* 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;
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<M, N> &operator -=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
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++) {
data[i][j] -= m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this);
}
@ -255,38 +287,50 @@ public:
/**
* uniform scaling
*/
Matrix<M, N> operator *(const float num) const {
Matrix<M, N> operator *(const float num) const
{
Matrix<M, N> 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<M, N> &operator *=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
Matrix<M, N> &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<Matrix<M, N>*>(this);
}
Matrix<M, N> operator /(const float num) const {
Matrix<M, N> operator /(const float num) const
{
Matrix<M, N> 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<M, N> &operator /=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
Matrix<M, N> &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<Matrix<M, N>*>(this);
}
@ -295,7 +339,8 @@ public:
* multiplication by another matrix
*/
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, N, P> Him(m.arm_mat.pData);
matrix::Matrix<float, M, P> Product = Me * Him;
@ -306,7 +351,8 @@ public:
/**
* 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<N, M> res(Me.transpose().data());
return res;
@ -315,7 +361,8 @@ public:
/**
* 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<M, N> 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<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));
return *this;
}
@ -376,7 +429,8 @@ public:
/**
* 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, N, 1> Vec(v.arm_col.pData);
matrix::Matrix<float, M, 1> 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<float, 9ul> d) {
set(static_cast<const float*>(d.data()));
void set(const boost::array<float, 9ul> d)
{
set(static_cast<const float *>(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]);

103
src/lib/mathlib/math/Quaternion.hpp
View File

@ -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];

225
src/lib/mathlib/math/Vector.hpp
View File

@ -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<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
bool operator ==(const Vector<N> &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<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
bool operator !=(const Vector<N> &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<N> &operator =(const Vector<N> &v) {
const Vector<N> &operator =(const Vector<N> &v)
{
memcpy(data, v.data, sizeof(data));
return *static_cast<const Vector<N>*>(this);
}
@ -164,11 +175,13 @@ public:
/**
* negation
*/
const Vector<N> operator -(void) const {
const Vector<N> operator -(void) const
{
Vector<N> 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<N> operator +(const Vector<N> &v) const {
const Vector<N> operator +(const Vector<N> &v) const
{
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];
}
return res;
}
@ -188,11 +203,13 @@ public:
/**
* subtraction
*/
const Vector<N> operator -(const Vector<N> &v) const {
const Vector<N> operator -(const Vector<N> &v) const
{
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];
}
return res;
}
@ -200,11 +217,13 @@ public:
/**
* uniform scaling
*/
const Vector<N> operator *(const float num) const {
const Vector<N> operator *(const float num) const
{
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;
}
return res;
}
@ -212,11 +231,13 @@ public:
/**
* uniform scaling
*/
const Vector<N> operator /(const float num) const {
const Vector<N> operator /(const float num) const
{
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;
}
return res;
}
@ -224,9 +245,11 @@ public:
/**
* addition
*/
const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
const Vector<N> &operator +=(const Vector<N> &v)
{
for (unsigned int i = 0; i < N; i++) {
data[i] += v.data[i];
}
return *static_cast<const Vector<N>*>(this);
}
@ -234,9 +257,11 @@ public:
/**
* subtraction
*/
const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
const Vector<N> &operator -=(const Vector<N> &v)
{
for (unsigned int i = 0; i < N; i++) {
data[i] -= v.data[i];
}
return *static_cast<const Vector<N>*>(this);
}
@ -244,9 +269,11 @@ public:
/**
* uniform scaling
*/
const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++)
const Vector<N> &operator *=(const float num)
{
for (unsigned int i = 0; i < N; i++) {
data[i] *= num;
}
return *static_cast<const Vector<N>*>(this);
}
@ -254,9 +281,11 @@ public:
/**
* uniform scaling
*/
const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++)
const Vector<N> &operator /=(const float num)
{
for (unsigned int i = 0; i < N; i++) {
data[i] /= num;
}
return *static_cast<const Vector<N>*>(this);
}
@ -264,11 +293,13 @@ public:
/**
* dot product
*/
float operator *(const Vector<N> &v) const {
float operator *(const Vector<N> &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<N> emult(const Vector<N> &v) const {
const Vector<N> emult(const Vector<N> &v) const
{
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];
}
return res;
}
@ -288,11 +321,13 @@ public:
/**
* 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;
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<N> normalized() const {
Vector<N> 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<N> &operator =(const Vector<N> &v) {
const Vector<N> &operator =(const Vector<N> &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<float, 2ul> d) {
set(static_cast<const float*>(d.data()));
void set(const boost::array<float, 2ul> d)
{
set(static_cast<const float *>(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<float, 3ul> d) {
set(static_cast<const float*>(d.data()));
void set(const boost::array<float, 3ul> d)
{
set(static_cast<const float *>(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<float, 4ul> d) {
set(static_cast<const float*>(d.data()));
void set(const boost::array<float, 4ul> d)
{
set(static_cast<const float *>(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;
}

73
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -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 <LeonardTHall@gmail.com>
#include <px4_defines.h>
@ -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

87
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -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 <LeonardTHall@gmail.com>
/// 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

5
src/lib/mathlib/math/test/test.cpp
View File

@ -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;

2
src/modules/uORB/uORBManager.hpp
View File

@ -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

4
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
View File

@ -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;