diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 195c7e0ee2..9ad7769d02 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 757fac1633..a48c1bbf28 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -41,6 +41,7 @@ #include #include // NAN +#include #include #include diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 940c9eeb5f..49f10bf1a9 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include using namespace matrix; diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp index f3a1269b98..2e07d81632 100644 --- a/src/drivers/vmount/output.cpp +++ b/src/drivers/vmount/output.cpp @@ -49,6 +49,7 @@ #include #include #include +#include namespace vmount { diff --git a/src/examples/uuv_example_app/uuv_example_app.cpp b/src/examples/uuv_example_app/uuv_example_app.cpp index 63f7582405..8d1cc46203 100644 --- a/src/examples/uuv_example_app/uuv_example_app.cpp +++ b/src/examples/uuv_example_app/uuv_example_app.cpp @@ -58,6 +58,7 @@ #include // internal libraries #include +#include #include // Include uORB and the required topics for this app diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 04a711837c..c690ad476b 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -41,6 +41,7 @@ #include "battery.h" #include +#include Battery::Battery() : ModuleParams(nullptr), diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 6b0e4f80f4..1ffc18150c 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -40,16 +40,6 @@ #include "math.h" #include "rotation.h" -__EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) -{ - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; - float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; - float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - - rot_matrix->from_euler(roll, pitch, yaw); -} - __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot) { @@ -59,8 +49,6 @@ get_rot_matrix(enum Rotation rot) math::radians((float)rot_lookup[rot].yaw)}}; } -#define HALF_SQRT_2 0.70710678118654757f - __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { @@ -72,8 +60,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) return; case ROTATION_YAW_45: { - tmp = HALF_SQRT_2 * (x - y); - y = HALF_SQRT_2 * (x + y); + tmp = M_SQRT1_2_F * (x - y); + y = M_SQRT1_2_F * (x + y); x = tmp; return; } @@ -84,8 +72,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (x - y); + tmp = -M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (x - y); x = tmp; return; } @@ -95,8 +83,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) return; case ROTATION_YAW_225: { - tmp = HALF_SQRT_2 * (y - x); - y = -HALF_SQRT_2 * (x + y); + tmp = M_SQRT1_2_F * (y - x); + y = -M_SQRT1_2_F * (x + y); x = tmp; return; } @@ -107,8 +95,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_YAW_315: { - tmp = HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (y - x); + tmp = M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (y - x); x = tmp; return; } @@ -119,8 +107,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (x - y); + tmp = M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (x - y); x = tmp; z = -z; return; } @@ -131,8 +119,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2 * (y - x); - y = HALF_SQRT_2 * (y + x); + tmp = M_SQRT1_2_F * (y - x); + y = M_SQRT1_2_F * (y + x); x = tmp; z = -z; return; } @@ -143,8 +131,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (y - x); + tmp = -M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (y - x); x = tmp; z = -z; return; } @@ -155,8 +143,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2 * (x - y); - y = -HALF_SQRT_2 * (x + y); + tmp = M_SQRT1_2_F * (x - y); + y = -M_SQRT1_2_F * (x + y); x = tmp; z = -z; return; } @@ -168,8 +156,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) case ROTATION_ROLL_90_YAW_45: { tmp = z; z = y; y = -tmp; - tmp = HALF_SQRT_2 * (x - y); - y = HALF_SQRT_2 * (x + y); + tmp = M_SQRT1_2_F * (x - y); + y = M_SQRT1_2_F * (x + y); x = tmp; return; } @@ -182,8 +170,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) case ROTATION_ROLL_90_YAW_135: { tmp = z; z = y; y = -tmp; - tmp = -HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (x - y); + tmp = -M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (x - y); x = tmp; return; } @@ -195,8 +183,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) case ROTATION_ROLL_270_YAW_45: { tmp = z; z = -y; y = tmp; - tmp = HALF_SQRT_2 * (x - y); - y = HALF_SQRT_2 * (x + y); + tmp = M_SQRT1_2_F * (x - y); + y = M_SQRT1_2_F * (x + y); x = tmp; return; } @@ -209,8 +197,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) case ROTATION_ROLL_270_YAW_135: { tmp = z; z = -y; y = tmp; - tmp = -HALF_SQRT_2 * (x + y); - y = HALF_SQRT_2 * (x - y); + tmp = -M_SQRT1_2_F * (x + y); + y = M_SQRT1_2_F * (x - y); x = tmp; return; } @@ -276,15 +264,15 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_PITCH_45: { - tmp = HALF_SQRT_2 * x + HALF_SQRT_2 * z; - z = HALF_SQRT_2 * z - HALF_SQRT_2 * x; + tmp = M_SQRT1_2_F * x + M_SQRT1_2_F * z; + z = M_SQRT1_2_F * z - M_SQRT1_2_F * x; x = tmp; return; } case ROTATION_PITCH_315: { - tmp = HALF_SQRT_2 * x - HALF_SQRT_2 * z; - z = HALF_SQRT_2 * z + HALF_SQRT_2 * x; + tmp = M_SQRT1_2_F * x - M_SQRT1_2_F * z; + z = M_SQRT1_2_F * z + M_SQRT1_2_F * x; x = tmp; return; } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index fedb06ee37..d228177477 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -42,6 +42,7 @@ #include #include +#include /** * Enum for board and external compass rotations. @@ -133,7 +134,6 @@ const rot_lookup_t rot_lookup[] = { /** * Get the rotation matrix */ -__EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); diff --git a/src/lib/drivers/device/integrator.h b/src/lib/drivers/device/integrator.h index 30ef7a4f92..45842c8c2d 100644 --- a/src/lib/drivers/device/integrator.h +++ b/src/lib/drivers/device/integrator.h @@ -43,6 +43,7 @@ #pragma once #include +#include class Integrator { diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp deleted file mode 100644 index 8cc286eac1..0000000000 --- a/src/lib/mathlib/math/Matrix.hpp +++ /dev/null @@ -1,544 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * Pavel Kirienko - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.hpp - * - * Matrix class - */ - -#ifndef MATRIX_HPP -#define MATRIX_HPP - -#include -#include - -#include "Vector.hpp" // Vector and eigen_matrix_instance - -#include "matrix/math.hpp" -#include - -namespace math -{ - -template -class __EXPORT Matrix; - -// MxN matrix with float elements -template -class __EXPORT MatrixBase -{ -public: - /** - * matrix data[row][col] - */ - float data[M][N]; - - /** - * struct for using arm_math functions - */ - eigen_matrix_instance arm_mat; - - /** - * trivial ctor - * Initializes the elements to zero. - */ - MatrixBase() : - data{}, - arm_mat{M, N, &data[0][0]} - { - } - - ~MatrixBase() = default; - - /** - * copyt ctor - */ - MatrixBase(const MatrixBase &m) : - arm_mat{M, N, &data[0][0]} - { - memcpy(data, m.data, sizeof(data)); - } - - MatrixBase(const float *d) : - arm_mat{M, N, &data[0][0]} - { - memcpy(data, d, sizeof(data)); - } - - MatrixBase(const float d[M][N]) : - arm_mat{M, N, &data[0][0]} - { - memcpy(data, d, sizeof(data)); - } - - /** - * set data - */ - void set(const float *d) - { - memcpy(data, d, sizeof(data)); - } - - /** - * set data - */ - 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) - { - for (unsigned i = 0; i < N; i++) { - data[row][i] = v.data[i]; - } - } - - /** - * set column from vector - */ - void set_col(unsigned int col, const Vector v) - { - for (unsigned i = 0; i < M; i++) { - data[i][col] = v.data[i]; - } - } - - /** - * access by index - */ - 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 - { - return data[row][col]; - } - - /** - * get rows number - */ - unsigned int get_rows() const - { - return M; - } - - /** - * get columns number - */ - 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]) { - return false; - } - } - } - - return true; - } - - /** - * 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]) { - return true; - } - } - } - - return false; - } - - /** - * set to value - */ - const Matrix &operator =(const Matrix &m) - { - memcpy(data, m.data, sizeof(data)); - return *static_cast*>(this); - } - - /** - * negation - */ - Matrix operator -() const - { - Matrix res; - - 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; - } - - /** - * addition - */ - Matrix operator +(const Matrix &m) const - { - Matrix res; - - 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++) { - data[i][j] += m.data[i][j]; - } - } - - return *static_cast*>(this); - } - - /** - * subtraction - */ - Matrix operator -(const Matrix &m) const - { - Matrix res; - - 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++) { - data[i][j] -= m.data[i][j]; - } - } - - return *static_cast*>(this); - } - - /** - * uniform scaling - */ - Matrix operator *(const float num) const - { - Matrix res; - - 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++) { - data[i][j] *= num; - } - } - - return *static_cast*>(this); - } - - Matrix operator /(const float num) const - { - Matrix res; - - 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++) { - data[i][j] /= num; - } - } - - return *static_cast*>(this); - } - - /** - * multiplication by another matrix - */ - template - 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; - Matrix res(Product.data()); - return res; - } - - /** - * transpose the matrix - */ - Matrix transposed() const - { - matrix::Matrix Me(this->arm_mat.pData); - Matrix res(Me.transpose().data()); - return res; - } - - /** - * invert the matrix - */ - Matrix inversed() const - { - matrix::SquareMatrix Me = matrix::Matrix(this->arm_mat.pData); - Matrix res(Me.I().data()); - return res; - } - - /** - * set zero matrix - */ - void zero() - { - memset(data, 0, sizeof(data)); - } - - /** - * set identity matrix - */ - void identity() - { - memset(data, 0, sizeof(data)); - unsigned int n = (M < N) ? M : N; - - for (unsigned int i = 0; i < n; i++) { - data[i][i] = 1; - } - } - - void print() - { - for (unsigned int i = 0; i < M; i++) { - printf("[ "); - - for (unsigned int j = 0; j < N; j++) { - printf("%.3f\t", (double)data[i][j]); - } - - printf(" ]\n"); - } - } -}; - -template -class __EXPORT Matrix : public MatrixBase -{ -public: - using MatrixBase::operator *; - - Matrix() : MatrixBase() {} - - Matrix(const Matrix &m) : MatrixBase(m) {} - - Matrix(const float *d) : MatrixBase(d) {} - - Matrix(const float d[M][N]) : MatrixBase(d) {} - - /** - * set to value - */ - const Matrix &operator =(const Matrix &m) - { - memcpy(this->data, m.data, sizeof(this->data)); - return *this; - } - - /** - * multiplication by a vector - */ - 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; - Vector res(Product.data()); - return res; - } -}; - -template <> -class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> -{ -public: - using MatrixBase<3, 3>::operator *; - - Matrix() : MatrixBase<3, 3>() {} - - Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} - - Matrix(const float *d) : MatrixBase<3, 3>(d) {} - - Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} - /** - * set data - */ - void set(const float d[9]) - { - memcpy(data, d, sizeof(data)); - } - -#if defined(__PX4_ROS) - /** - * set data from boost::array - */ - 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) - { - memcpy(this->data, m.data, sizeof(this->data)); - return *this; - } - - /** - * multiplication by a vector - */ - 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]); - return res; - } - - /** - * 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) - { - float cp = cosf(pitch); - float sp = sinf(pitch); - float sr = sinf(roll); - float cr = cosf(roll); - float sy = sinf(yaw); - float cy = cosf(yaw); - - data[0][0] = cp * cy; - data[0][1] = (sr * sp * cy) - (cr * sy); - data[0][2] = (cr * sp * cy) + (sr * sy); - data[1][0] = cp * sy; - data[1][1] = (sr * sp * sy) + (cr * cy); - data[1][2] = (cr * sp * sy) - (sr * cy); - data[2][0] = -sp; - data[2][1] = sr * cp; - data[2][2] = cr * cp; - } - - /** - * get euler angles from rotation matrix - */ - Vector<3> to_euler() const - { - Vector<3> euler; - euler.data[1] = asinf(-data[2][0]); - - if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { - euler.data[0] = 0.0f; - euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; - - } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { - euler.data[0] = 0.0f; - euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; - - } else { - euler.data[0] = atan2f(data[2][1], data[2][2]); - euler.data[2] = atan2f(data[1][0], data[0][0]); - } - - return euler; - } -}; - -} - -#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp deleted file mode 100644 index 34e1ecab3e..0000000000 --- a/src/lib/mathlib/math/Quaternion.hpp +++ /dev/null @@ -1,310 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Quaternion.hpp - * - * Quaternion class - * - * @author Anton Babushkin - * @author Pavel Kirienko - * @author Lorenz Meier - */ - -#ifndef QUATERNION_HPP -#define QUATERNION_HPP - -#include - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class __EXPORT Quaternion : public Vector<4> -{ -public: - /** - * trivial ctor - */ - Quaternion() : Vector<4>() {} - - /** - * copy ctor - */ - Quaternion(const Quaternion &q) : Vector<4>(q) {} - - /** - * casting from vector - */ - Quaternion(const Vector<4> &v) : Vector<4>(v) {} - - /** - * setting ctor - */ - Quaternion(const float d[4]) : Vector<4>(d) {} - - /** - * setting ctor - */ - Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} - - using Vector<4>::operator *; - - /** - * multiplication - */ - 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], - data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], - data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); - } - - /** - * division - */ - 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 - ); - } - - /** - * derivative - */ - 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], - data[2], data[3], data[0], -data[1], - data[3], -data[2], data[1], data[0] - }; - Matrix<4, 4> Q(dataQ); - Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); - return Q * v * 0.5f; - } - - /** - * conjugate - */ - Quaternion conjugated() const - { - return Quaternion(data[0], -data[1], -data[2], -data[3]); - } - - /** - * inversed - */ - Quaternion inversed() const - { - float norm = length_squared(); - return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm); - } - - /** - * conjugation - */ - 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] * 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) - ); - } - - /** - * conjugation with inversed quaternion - */ - 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] * 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) - ); - } - - /** - * imaginary part of quaternion - */ - 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) - { - double cosPhi_2 = cos(double(roll) / 2.0); - double sinPhi_2 = sin(double(roll) / 2.0); - double cosTheta_2 = cos(double(pitch) / 2.0); - double sinTheta_2 = sin(double(pitch) / 2.0); - double cosPsi_2 = cos(double(yaw) / 2.0); - double sinPsi_2 = sin(double(yaw) / 2.0); - - /* operations executed in double to avoid loss of precision through - * consecutive multiplications. Result stored as float. - */ - data[0] = static_cast(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); - data[1] = static_cast(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); - data[2] = static_cast(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); - data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); - } - - /** - * simplified version of the above method to create quaternion representing rotation only by yaw - */ - void from_yaw(float yaw) - { - data[0] = cosf(yaw / 2.0f); - data[1] = 0.0f; - data[2] = 0.0f; - data[3] = sinf(yaw / 2.0f); - } - - /** - * create Euler angles vector from the quaternion - */ - 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])) - ); - } - - /** - * 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) - { - 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; - s = 0.5f / s; - 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); - 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; - data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s; - data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s; - } - } - - /** - * create rotation matrix for the quaternion - */ - Matrix<3, 3> to_dcm() const - { - Matrix<3, 3> R; - float aSq = data[0] * data[0]; - float bSq = data[1] * data[1]; - float cSq = data[2] * data[2]; - float dSq = data[3] * data[3]; - R.data[0][0] = aSq + bSq - cSq - dSq; - R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); - R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); - R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); - R.data[1][1] = aSq - bSq + cSq - dSq; - R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); - R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); - R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); - R.data[2][2] = aSq - bSq - cSq + dSq; - return R; - } -}; - -} - -#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp deleted file mode 100644 index d13c4cd152..0000000000 --- a/src/lib/mathlib/math/Vector.hpp +++ /dev/null @@ -1,608 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * Pavel Kirienko - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.hpp - * - * Vector class - */ - -#ifndef VECTOR_HPP -#define VECTOR_HPP - -#include -#include -#include - -#include - -#include - -namespace math -{ - -template -class __EXPORT Vector; - -template -class __EXPORT VectorBase -{ -public: - /** - * vector data - */ - float data[N]; - - /** - * struct for using arm_math functions, represents column vector - */ - eigen_matrix_instance arm_col; - - - /** - * trivial ctor - * initializes elements to zero - */ - VectorBase() : - data{}, - arm_col{N, 1, &data[0]} - { - - } - - ~VectorBase() = default; - - /** - * copy ctor - */ - VectorBase(const VectorBase &v) : - arm_col{N, 1, &data[0]} - { - memcpy(data, v.data, sizeof(data)); - } - - /** - * setting ctor - */ - VectorBase(const float d[N]) : - arm_col{N, 1, &data[0]} - { - memcpy(data, d, sizeof(data)); - } - - /** - * set data - */ - void set(const float d[N]) - { - memcpy(data, d, sizeof(data)); - } - - /** - * access to elements by index - */ - float &operator()(const unsigned int i) - { - return data[i]; - } - - /** - * access to elements by index - */ - float operator()(const unsigned int i) const - { - return data[i]; - } - - /** - * get vector size - */ - 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]) { - return false; - } - } - - return true; - } - - /** - * test for inequality - */ - bool operator !=(const Vector &v) const - { - for (unsigned int i = 0; i < N; i++) { - if (data[i] != v.data[i]) { - return true; - } - } - - return false; - } - - /** - * set to value - */ - const Vector &operator =(const Vector &v) - { - memcpy(data, v.data, sizeof(data)); - return *static_cast*>(this); - } - - /** - * negation - */ - const Vector operator -() const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = -data[i]; - } - - return res; - } - - /** - * addition - */ - const Vector operator +(const Vector &v) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] + v.data[i]; - } - - return res; - } - - /** - * subtraction - */ - const Vector operator -(const Vector &v) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] - v.data[i]; - } - - return res; - } - - /** - * uniform scaling - */ - const Vector operator *(const float num) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] * num; - } - - return res; - } - - /** - * uniform scaling - */ - const Vector operator /(const float num) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] / num; - } - - return res; - } - - /** - * addition - */ - const Vector &operator +=(const Vector &v) - { - for (unsigned int i = 0; i < N; i++) { - data[i] += v.data[i]; - } - - return *static_cast*>(this); - } - - /** - * subtraction - */ - const Vector &operator -=(const Vector &v) - { - for (unsigned int i = 0; i < N; i++) { - data[i] -= v.data[i]; - } - - return *static_cast*>(this); - } - - /** - * uniform scaling - */ - const Vector &operator *=(const float num) - { - for (unsigned int i = 0; i < N; i++) { - data[i] *= num; - } - - return *static_cast*>(this); - } - - /** - * uniform scaling - */ - const Vector &operator /=(const float num) - { - for (unsigned int i = 0; i < N; i++) { - data[i] /= num; - } - - return *static_cast*>(this); - } - - /** - * dot product - */ - float operator *(const Vector &v) const - { - float res = 0.0f; - - for (unsigned int i = 0; i < N; i++) { - res += data[i] * v.data[i]; - } - - return res; - } - - /** - * element by element multiplication - */ - const Vector emult(const Vector &v) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] * v.data[i]; - } - - return res; - } - - /** - * element by element division - */ - const Vector edivide(const Vector &v) const - { - Vector res; - - for (unsigned int i = 0; i < N; i++) { - res.data[i] = data[i] / v.data[i]; - } - - return res; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const - { - float res = 0.0f; - - for (unsigned int i = 0; i < N; i++) { - res += data[i] * data[i]; - } - - return res; - } - - /** - * gets the length of this vector - */ - float length() const - { - float res = 0.0f; - - for (unsigned int i = 0; i < N; i++) { - res += data[i] * data[i]; - } - - return sqrtf(res); - } - - /** - * normalizes this vector - */ - void normalize() - { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector normalized() const - { - return *this / length(); - } - - /** - * set zero vector - */ - void zero() - { - memset(data, 0, sizeof(data)); - } - - void print() - { - printf("[ "); - - for (unsigned int i = 0; i < N; i++) { - printf("%.3f\t", (double)data[i]); - } - - printf("]\n"); - } -}; - -template -class __EXPORT Vector : public VectorBase -{ -public: - Vector() : VectorBase() {} - - Vector(const Vector &v) : VectorBase(v) {} - - Vector(const float d[N]) : VectorBase(d) {} - - /** - * set to value - */ - const Vector &operator =(const Vector &v) - { - memcpy(this->data, v.data, sizeof(this->data)); - return *this; - } -}; - -template <> -class __EXPORT Vector<2> : public VectorBase<2> -{ -public: - Vector() : VectorBase<2>() {} - - // simple copy is 1.6 times faster than memcpy - Vector(const Vector<2> &v) : VectorBase<2>() - { - data[0] = v.data[0]; - data[1] = v.data[1]; - } - - Vector(const float d[2]) : VectorBase<2>() - { - data[0] = d[0]; - data[1] = d[1]; - } - - Vector(const float x, const float y) : VectorBase<2>() - { - data[0] = x; - data[1] = y; - } - - /** - * set data - */ - void set(const float d[2]) - { - data[0] = d[0]; - data[1] = d[1]; - } -#if defined(__PX4_ROS) - /** - * set data from boost::array - */ - void set(const boost::array d) - { - set(static_cast(d.data())); - } -#endif - /** - * set to value - */ - 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 - { - return data[0] * v.data[1] - data[1] * v.data[0]; - } -}; - -template <> -class __EXPORT Vector<3> : public VectorBase<3> -{ -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++) { - data[i] = v.data[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>() - { - data[0] = x; - data[1] = y; - data[2] = z; - } -#if defined(__PX4_ROS) - /** - * set data from boost::array - */ - 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++) { - data[i] = d[i]; - } - } - - /** - * set to value - */ - 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 - { - return Vector<3>( - data[1] * v.data[2] - data[2] * v.data[1], - data[2] * v.data[0] - data[0] * v.data[2], - data[0] * v.data[1] - data[1] * v.data[0] - ); - } -}; - -template <> -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++) { - data[i] = v.data[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() - { - data[0] = x0; - data[1] = x1; - data[2] = x2; - data[3] = x3; - } -#if defined(__PX4_ROS) - /** - * set data from boost::array - */ - 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++) { - data[i] = d[i]; - } - } - - /** - * set to value - */ - const Vector<4> &operator =(const Vector<4> &v) - { - for (unsigned int i = 0; i < 4; i++) { - data[i] = v.data[i]; - } - - return *this; - } -}; - -} - -#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf deleted file mode 100644 index eb67a4bfcf..0000000000 Binary files a/src/lib/mathlib/math/nasa_rotation_def.pdf and /dev/null differ diff --git a/src/lib/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce deleted file mode 100644 index c3fba4729d..0000000000 --- a/src/lib/mathlib/math/test_math.sce +++ /dev/null @@ -1,63 +0,0 @@ -clc -clear -function out = float_truncate(in, digits) - out = round(in*10^digits) - out = out/10^digits -endfunction - -phi = 0.1 -theta = 0.2 -psi = 0.3 - -cosPhi = cos(phi) -cosPhi_2 = cos(phi/2) -sinPhi = sin(phi) -sinPhi_2 = sin(phi/2) - -cosTheta = cos(theta) -cosTheta_2 = cos(theta/2) -sinTheta = sin(theta) -sinTheta_2 = sin(theta/2) - -cosPsi = cos(psi) -cosPsi_2 = cos(psi/2) -sinPsi = sin(psi) -sinPsi_2 = sin(psi/2) - -C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; - cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; - -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] - -disp(C_nb) -//C_nb = float_truncate(C_nb,3) -//disp(C_nb) - -theta = asin(-C_nb(3,1)) -phi = atan(C_nb(3,2), C_nb(3,3)) -psi = atan(C_nb(2,1), C_nb(1,1)) -printf('phi %f\n', phi) -printf('theta %f\n', theta) -printf('psi %f\n', psi) - -q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; - sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; - cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; - cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] - -//q = float_truncate(q,3) - -a = q(1) -b = q(2) -c = q(3) -d = q(4) -printf('q: %f %f %f %f\n', a, b, c, d) -a2 = a*a -b2 = b*b -c2 = c*c -d2 = d*d - -C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); - 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); - 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] - -disp(C2_nb) diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 50cb12be63..40bb8f3e0b 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,9 +41,6 @@ #ifdef __cplusplus -#include "math/Vector.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" #include "math/Limits.hpp" #include "math/Functions.hpp" #include "math/matrix_alg.h" diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 3b654256fb..a284656ea6 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -40,6 +40,7 @@ #include "mixer.h" #include +#include #include diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 891b5a2da0..ab734775e7 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -38,7 +38,7 @@ #pragma once #include -#include "matrix/Matrix.hpp" +#include #include #include #include diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 12c7482bb9..55a39cfb27 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index b8f5a1d211..ab1ab1b2f8 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -270,13 +271,10 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & float fitness = _fitness; float fit1 = 0.0f, fit2 = 0.0f; - float JTJ[16]; - float JTJ2[16]; - float JTFI[4]; + matrix::SquareMatrix JTJ; + matrix::SquareMatrix JTJ2; + float JTFI[4] = {}; float residual = 0.0f; - memset(JTJ, 0, sizeof(JTJ)); - memset(JTJ2, 0, sizeof(JTJ2)); - memset(JTFI, 0, sizeof(JTFI)); // Gauss Newton Part common for all kind of extensions including LM for (uint16_t k = 0; k < _samples_collected; k++) { @@ -299,8 +297,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & for (uint8_t i = 0; i < 4; i++) { // compute JTJ for (uint8_t j = 0; j < 4; j++) { - JTJ[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; - JTJ2[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM + JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j]; + JTJ2(i, j) += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM } JTFI[i] += sphere_jacob[i] * residual; @@ -315,22 +313,22 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & memcpy(fit2_params, fit1_params, sizeof(fit1_params)); for (uint8_t i = 0; i < 4; i++) { - JTJ[i * 4 + i] += _sphere_lambda; - JTJ2[i * 4 + i] += _sphere_lambda / lma_damping; + JTJ(i, i) += _sphere_lambda; + JTJ2(i, i) += _sphere_lambda / lma_damping; } - if (!inverse4x4(JTJ, JTJ)) { + if (!JTJ.I(JTJ)) { return -1; } - if (!inverse4x4(JTJ2, JTJ2)) { + if (!JTJ2.I(JTJ2)) { return -1; } for (uint8_t row = 0; row < 4; row++) { for (uint8_t col = 0; col < 4; col++) { - fit1_params[row] -= JTFI[col] * JTJ[row * 4 + col]; - fit2_params[row] -= JTFI[col] * JTJ2[row * 4 + col]; + fit1_params[row] -= JTFI[col] * JTJ(row, col); + fit2_params[row] -= JTFI[col] * JTJ2(row, col); } } @@ -395,15 +393,13 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa const float lma_damping = 10.0f; float _samples_collected = size; float fitness = _fitness; - float fit1 = 0.0f, fit2 = 0.0f; + float fit1 = 0.0f; + float fit2 = 0.0f; - float JTJ[81]; - float JTJ2[81]; - float JTFI[9]; + float JTJ[81] = {}; + float JTJ2[81] = {}; + float JTFI[9] = {}; float residual = 0.0f; - memset(JTJ, 0, sizeof(JTJ)); - memset(JTJ2, 0, sizeof(JTJ2)); - memset(JTFI, 0, sizeof(JTFI)); float ellipsoid_jacob[9]; // Gauss Newton Part common for all kind of extensions including LM @@ -461,8 +457,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa return -1; } - - for (uint8_t row = 0; row < 9; row++) { for (uint8_t col = 0; col < 9; col++) { fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col]; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 32a51e76f8..b72d16e8cb 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index 70204d2f67..90f06cbe4d 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -49,6 +49,7 @@ #include #include #include +#include namespace runwaytakeoff { diff --git a/src/modules/gnd_att_control/CMakeLists.txt b/src/modules/gnd_att_control/CMakeLists.txt index 46c0ea8d7c..f5864fe0f7 100644 --- a/src/modules/gnd_att_control/CMakeLists.txt +++ b/src/modules/gnd_att_control/CMakeLists.txt @@ -37,5 +37,4 @@ px4_add_module( COMPILE_FLAGS SRCS GroundRoverAttitudeControl.cpp - DEPENDS ) diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp index e74c6d8cb7..017e9ccc1b 100644 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index c82a8d267d..b99eafd0a4 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace px4 { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3386642b7b..1390185d67 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp b/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp index 7f8999b7da..573eabc19d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp @@ -42,6 +42,7 @@ #include #include #include +#include extern "C" __EXPORT int mc_pos_control_tests_main(int argc, char *argv[]); diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 540c6994b2..e3d0000551 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -40,11 +40,12 @@ #pragma once -#include - #include "navigator_mode.h" #include "mission_block.h" +#include +#include + #include #include diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 986dfa38a2..00a07b59ea 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -44,6 +44,7 @@ #include #include +#include #include "common.h" diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 9d92b932dd..f040bf1da0 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -242,9 +242,9 @@ void Tailsitter::update_transition_state() _v_att_sp->roll_body = 0.0f; _v_att_sp->yaw_body = _yaw_transition; - math::Quaternion q_sp; - q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); + matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); + q_sp.copyTo(_v_att_sp->q_d); + _v_att_sp->q_d_valid = true; } void Tailsitter::waiting_on_tecs() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index b776a4cea6..917e67ff81 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -58,7 +58,8 @@ #include #include #include -#include +#include +#include #include #include diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp index f6088a9a4b..a939987401 100644 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ b/src/modules/wind_estimator/wind_estimator_main.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ #include +#include #include -#include #include #include #include diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index b43cfa30d3..5f3992c4f6 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include "tests_main.h" @@ -64,7 +65,6 @@ public: private: bool testVector2(); bool testVector3(); - bool testVector4(); bool testVector10(); bool testMatrix3x3(); bool testMatrix10x10(); @@ -82,21 +82,21 @@ using namespace math; bool MathlibTest::testVector2() { { - Vector<2> v; - Vector<2> v1(1.0f, 2.0f); - Vector<2> v2(1.0f, -1.0f); + matrix::Vector2f v; + matrix::Vector2f v1(1.0f, 2.0f); + matrix::Vector2f v2(1.0f, -1.0f); float data[2] = {1.0f, 2.0f}; - TEST_OP("Constructor Vector<2>()", Vector<2> v3); - TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1); ut_assert_true(v3 == v1); v3.zero()); - TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); - TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); - TEST_OP("Vector<2> = Vector<2>", v = v1); - TEST_OP("Vector<2> + Vector<2>", v + v1); - TEST_OP("Vector<2> - Vector<2>", v - v1); - TEST_OP("Vector<2> += Vector<2>", v += v1); - TEST_OP("Vector<2> -= Vector<2>", v -= v1); - TEST_OP("Vector<2> * Vector<2>", v * v1); - TEST_OP("Vector<2> %% Vector<2>", v1 % v2); + TEST_OP("Constructor matrix::Vector2f()", matrix::Vector2f v3); + TEST_OP("Constructor matrix::Vector2f(matrix::Vector2f)", matrix::Vector2f v3(v1); ut_assert_true(v3 == v1); v3.zero()); + TEST_OP("Constructor matrix::Vector2f(float[])", matrix::Vector2f v3(data)); + TEST_OP("Constructor matrix::Vector2f(float, float)", matrix::Vector2f v3(1.0f, 2.0f)); + TEST_OP("matrix::Vector2f = matrix::Vector2f", v = v1); + TEST_OP("matrix::Vector2f + matrix::Vector2f", v + v1); + TEST_OP("matrix::Vector2f - matrix::Vector2f", v - v1); + TEST_OP("matrix::Vector2f += matrix::Vector2f", v += v1); + TEST_OP("matrix::Vector2f -= matrix::Vector2f", v -= v1); + TEST_OP("matrix::Vector2f * matrix::Vector2f", v * v1); + TEST_OP("matrix::Vector2f %% matrix::Vector2f", v1 % v2); } return true; } @@ -105,70 +105,33 @@ bool MathlibTest::testVector3() { { - Vector<3> v; - Vector<3> v1(1.0f, 2.0f, 0.0f); - Vector<3> v2(1.0f, -1.0f, 2.0f); + matrix::Vector3f v; + matrix::Vector3f v1(1.0f, 2.0f, 0.0f); + matrix::Vector3f v2(1.0f, -1.0f, 2.0f); float data[3] = {1.0f, 2.0f, 3.0f}; - TEST_OP("Constructor Vector<3>()", Vector<3> v3); - TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1); ut_assert_true(v3 == v1); v3.zero()); - TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); - TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); - TEST_OP("Vector<3> = Vector<3>", v = v1); - TEST_OP("Vector<3> + Vector<3>", v + v1); - TEST_OP("Vector<3> - Vector<3>", v - v1); - TEST_OP("Vector<3> += Vector<3>", v += v1); - TEST_OP("Vector<3> -= Vector<3>", v -= v1); - TEST_OP("Vector<3> * float", v1 * 2.0f); - TEST_OP("Vector<3> / float", v1 / 2.0f); - TEST_OP("Vector<3> *= float", v1 *= 2.0f); - TEST_OP("Vector<3> /= float", v1 /= 2.0f); - TEST_OP("Vector<3> * Vector<3>", v * v1); - TEST_OP("Vector<3> %% Vector<3>", v1 % v2); - TEST_OP("Vector<3> length", v1.length()); - TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Constructor matrix::Vector3f()", matrix::Vector3f v3); + TEST_OP("Constructor matrix::Vector3f(matrix::Vector3f)", matrix::Vector3f v3(v1); ut_assert_true(v3 == v1); v3.zero()); + TEST_OP("Constructor matrix::Vector3f(float[])", matrix::Vector3f v3(data)); + TEST_OP("Constructor matrix::Vector3f(float, float, float)", matrix::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("matrix::Vector3f = matrix::Vector3f", v = v1); + TEST_OP("matrix::Vector3f + matrix::Vector3f", v + v1); + TEST_OP("matrix::Vector3f - matrix::Vector3f", v - v1); + TEST_OP("matrix::Vector3f += matrix::Vector3f", v += v1); + TEST_OP("matrix::Vector3f -= matrix::Vector3f", v -= v1); + TEST_OP("matrix::Vector3f * float", v1 * 2.0f); + TEST_OP("matrix::Vector3f / float", v1 / 2.0f); + TEST_OP("matrix::Vector3f *= float", v1 *= 2.0f); + TEST_OP("matrix::Vector3f /= float", v1 /= 2.0f); + TEST_OP("matrix::Vector3f * matrix::Vector3f", v * v1); + TEST_OP("matrix::Vector3f %% matrix::Vector3f", v1 % v2); + TEST_OP("matrix::Vector3f length", v1.length()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" // Need pragma here instead of moving variable out of TEST_OP and just reference because // TEST_OP measures performance of vector operations. - TEST_OP("Vector<3> element read", volatile float a = v1(0)); - TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("matrix::Vector3f element read", volatile float a = v1(0)); #pragma GCC diagnostic pop - TEST_OP("Vector<3> element write", v1(0) = 1.0f); - TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); - } - return true; -} - -bool MathlibTest::testVector4() -{ - { - Vector<4> v; - Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); - Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); - float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; - TEST_OP("Constructor Vector<4>()", Vector<4> v3); - TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1); ut_assert_true(v3 == v1); v3.zero()); - TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); - TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); - TEST_OP("Vector<4> = Vector<4>", v = v1); - TEST_OP("Vector<4> + Vector<4>", v + v1); - TEST_OP("Vector<4> - Vector<4>", v - v1); - TEST_OP("Vector<4> += Vector<4>", v += v1); - TEST_OP("Vector<4> -= Vector<4>", v -= v1); - TEST_OP("Vector<4> * Vector<4>", v * v1); - } - return true; -} - -bool MathlibTest::testVector10() -{ - { - Vector<10> v1; - v1.zero(); - float data[10]; - TEST_OP("Constructor Vector<10>()", Vector<10> v3); - TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1); ut_assert_true(v3 == v1); v3.zero()); - TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + TEST_OP("matrix::Vector3f element write", v1(0) = 1.0f); } return true; } @@ -176,30 +139,14 @@ bool MathlibTest::testVector10() bool MathlibTest::testMatrix3x3() { { - Matrix<3, 3> m1; + matrix::Matrix3f m1; m1.identity(); - Matrix<3, 3> m2; + matrix::Matrix3f m2; m2.identity(); - Vector<3> v1(1.0f, 2.0f, 0.0f); - TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); - TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); - TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); - } - return true; -} - -bool MathlibTest::testMatrix10x10() -{ - { - Matrix<10, 10> m1; - m1.identity(); - Matrix<10, 10> m2; - m2.identity(); - Vector<10> v1; - v1.zero(); - TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); - TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); - TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + matrix::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("matrix::Matrix3f * matrix::Vector3f", m1 * v1); + TEST_OP("matrix::Matrix3f + matrix::Matrix3f", m1 + m2); + TEST_OP("matrix::Matrix3f * matrix::Matrix3f", m1 * m2); } return true; } @@ -215,12 +162,12 @@ bool MathlibTest::testMatrixNonsymmetric() float data2[2][3] = {{2, 4, 6}, {8, 10, 12}}; float data3[2][3] = {{3, 6, 9}, {12, 15, 18}}; - Matrix<2, 3> m1(data1); - Matrix<2, 3> m2(data2); - Matrix<2, 3> m3(data3); + matrix::Matrix m1(data1); + matrix::Matrix m2(data2); + matrix::Matrix m3(data3); if (m1 + m2 != m3) { - PX4_ERR("Matrix<2, 3> + Matrix<2, 3> failed!"); + PX4_ERR("matrix::Matrix + matrix::Matrix failed!"); (m1 + m2).print(); printf("!=\n"); m3.print(); @@ -230,7 +177,7 @@ bool MathlibTest::testMatrixNonsymmetric() ut_assert("m1 + m2 == m3", m1 + m2 == m3); if (m3 - m2 != m1) { - PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!"); + PX4_ERR("matrix::Matrix - matrix::Matrix failed!"); (m3 - m2).print(); printf("!=\n"); m1.print(); @@ -242,7 +189,7 @@ bool MathlibTest::testMatrixNonsymmetric() m1 += m2; if (m1 != m3) { - PX4_ERR("Matrix<2, 3> += Matrix<2, 3> failed!"); + PX4_ERR("matrix::Matrix += matrix::Matrix failed!"); m1.print(); printf("!=\n"); m3.print(); @@ -252,10 +199,10 @@ bool MathlibTest::testMatrixNonsymmetric() ut_assert("m1 == m3", m1 == m3); m1 -= m2; - Matrix<2, 3> m1_orig(data1); + matrix::Matrix m1_orig(data1); if (m1 != m1_orig) { - PX4_ERR("Matrix<2, 3> -= Matrix<2, 3> failed!"); + PX4_ERR("matrix::Matrix -= matrix::Matrix failed!"); m1.print(); printf("!=\n"); m1_orig.print(); @@ -271,9 +218,9 @@ bool MathlibTest::testMatrixNonsymmetric() bool MathlibTest::testRotationMatrixQuaternion() { // test conversion rotation matrix to quaternion and back - math::Matrix<3, 3> R_orig; - math::Matrix<3, 3> R; - math::Quaternion q; + matrix::Dcmf R_orig; + matrix::Dcmf R; + matrix::Quatf q; float diff = 0.2f; float tol = 0.00001f; @@ -282,13 +229,13 @@ bool MathlibTest::testRotationMatrixQuaternion() for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R_orig.from_euler(roll, pitch, yaw); - q.from_dcm(R_orig); - R = q.to_dcm(); + R_orig = matrix::Eulerf(roll, pitch, yaw); + q = R_orig; + R = q; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol); + ut_assert("matrix::Quatf method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig(i, j) - R(i, j)) < tol); } } } @@ -303,16 +250,16 @@ bool MathlibTest::testQuaternionfrom_dcm() { // test against some known values float tol = 0.0001f; - math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; - math::Matrix<3, 3> R_orig; + matrix::Matrix3f R_orig; R_orig.identity(); - math::Quaternion q; + matrix::Quatf q; q.from_dcm(R_orig); for (unsigned i = 0; i < 4; i++) { - ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol); + ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol); } return true; @@ -321,33 +268,33 @@ bool MathlibTest::testQuaternionfrom_dcm() bool MathlibTest::testQuaternionfrom_euler() { float tol = 0.0001f; - math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; - math::Matrix<3, 3> R_orig; + matrix::Matrix3f R_orig; R_orig.identity(); - math::Quaternion q; + matrix::Quatf q; q.from_dcm(R_orig); - q_true.from_euler(0.3f, 0.2f, 0.1f); + q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f); q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; for (unsigned i = 0; i < 4; i++) { - ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol); + ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol); } - q_true.from_euler(-1.5f, -0.2f, 0.5f); + q_true = matrix::Eulerf(-1.5f, -0.2f, 0.5f); q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; for (unsigned i = 0; i < 4; i++) { - ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol); + ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol); } - q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q_true = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; for (unsigned i = 0; i < 4; i++) { - ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol); + ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol); } return true; @@ -356,26 +303,26 @@ bool MathlibTest::testQuaternionfrom_euler() bool MathlibTest::testQuaternionRotate() { // test quaternion method "rotate" (rotate vector by quaternion) - Vector<3> vector = {1.0f, 1.0f, 1.0f}; - Vector<3> vector_q; - Vector<3> vector_r; - Quaternion q; - Matrix<3, 3> R; + matrix::Vector3f vector = {1.0f, 1.0f, 1.0f}; + matrix::Vector3f vector_q; + matrix::Vector3f vector_r; + matrix::Quatf q; + matrix::Dcmf R; float diff = 0.2f; float tol = 0.00001f; - //PX4_INFO("Quaternion vector rotation method test."); + //PX4_INFO("matrix::Quatf vector rotation method test."); for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R.from_euler(roll, pitch, yaw); - q.from_euler(roll, pitch, yaw); + R = matrix::Eulerf(roll, pitch, yaw); + q = matrix::Eulerf(roll, pitch, yaw); vector_r = R * vector; vector_q = q.conjugate(vector); for (int i = 0; i < 3; i++) { - ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol); + ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol); } } } @@ -384,36 +331,36 @@ bool MathlibTest::testQuaternionRotate() // test some values calculated with matlab tol = 0.0001f; - q.from_euler(M_PI_2_F, 0.0f, 0.0f); + q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f); vector_q = q.conjugate(vector); - Vector<3> vector_true = {1.00f, -1.00f, 1.00f}; + matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f}; for (unsigned i = 0; i < 3; i++) { - ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); + ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); } - q.from_euler(0.3f, 0.2f, 0.1f); + q = matrix::Eulerf(0.3f, 0.2f, 0.1f); vector_q = q.conjugate(vector); vector_true = {1.1566, 0.7792, 1.0273}; for (unsigned i = 0; i < 3; i++) { - ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); + ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); } - q.from_euler(-1.5f, -0.2f, 0.5f); + q = matrix::Eulerf(-1.5f, -0.2f, 0.5f); vector_q = q.conjugate(vector); vector_true = {0.5095, 1.4956, -0.7096}; for (unsigned i = 0; i < 3; i++) { - ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); + ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); } - q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); + q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); vector_q = q.conjugate(vector); vector_true = { -1.3660, 0.3660, 1.0000}; for (unsigned i = 0; i < 3; i++) { - ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); + ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol); } return true; @@ -423,10 +370,7 @@ bool MathlibTest::run_tests() { ut_run_test(testVector2); ut_run_test(testVector3); - ut_run_test(testVector4); - ut_run_test(testVector10); ut_run_test(testMatrix3x3); - ut_run_test(testMatrix10x10); ut_run_test(testMatrixNonsymmetric); ut_run_test(testRotationMatrixQuaternion); ut_run_test(testQuaternionfrom_dcm);