From a83e3cd22276109301678c204e83050483200d6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 19:33:47 +0400 Subject: New mathlib, WIP --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mathlib.cpp | 114 ++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 117 insertions(+) create mode 100644 src/systemcmds/tests/test_mathlib.cpp (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d3..20182c5d8 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 000000000..13e6dad51 --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 test_mathlib.cpp + * + * Mathlib test + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + Matrix3f m; + m.identity(); + Matrix3f m1; + Matrix<3,3> mq; + mq.identity(); + Matrix<3,3> mq1; + m1(0, 0) = 5.0; + Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); + Vector3f v1; + + unsigned int n = 60000; + + hrt_abstime t0, t1; + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + v1 = m * v; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mq1 = mq * mq; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); + mq1.dump(); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + Matrix<4,4> mn; + mn(0, 0) = 2.0f; + mn(1, 0) = 3.0f; + for (int i = 0; i < mn.getRows(); i++) { + for (int j = 0; j < mn.getCols(); j++) { + printf("%.3f ", mn(i, j)); + } + printf("\n"); + } + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88..e2105f8ee 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18..fd026d70e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 23:01:02 +0400 Subject: mathlib: fixes and improvements, WIP --- src/lib/mathlib/math/Matrix.hpp | 38 ++++++++++----- src/lib/mathlib/math/Matrix3.hpp | 12 ++++- src/lib/mathlib/math/Vector.hpp | 90 ++++++++++++++++++++++++++--------- src/systemcmds/tests/test_mathlib.cpp | 80 ++++++++++++++++++++----------- 4 files changed, 157 insertions(+), 63 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f05360a19..1849f58be 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -34,9 +34,9 @@ ****************************************************************************/ /** - * @file Matrix3.hpp + * @file Matrix.hpp * - * 3x3 Matrix + * Generic Matrix */ #ifndef MATRIX_HPP @@ -203,6 +203,24 @@ public: return res; } + /** + * transpose the matrix + */ + Matrix transposed(void) const { + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix inversed(void) const { + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + /** * setup the identity matrix */ @@ -224,6 +242,8 @@ public: template class Matrix : public MatrixBase { public: + using MatrixBase::operator *; + /** * set to value */ @@ -235,18 +255,18 @@ public: /** * multiplication by a vector */ - /* Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } - */ }; template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: + using MatrixBase<3, 3>::operator *; + /** * set to value */ @@ -258,15 +278,11 @@ public: /** * multiplication by a vector */ - /* Vector<3> operator *(const Vector<3> &v) const { - Vector<3> res; - res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); - res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); - res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); - return res; + return Vector<3>(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]); } - */ }; } diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp index 3236cd3d1..c7eb67ba7 100644 --- a/src/lib/mathlib/math/Matrix3.hpp +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -74,14 +74,15 @@ public: * setting ctor */ Matrix3(const T d[3][3]) { - memcpy(data, d, sizeof(data)); arm_mat = {3, 3, &data[0][0]}; + memcpy(data, d, sizeof(data)); } /** * setting ctor */ Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + arm_mat = {3, 3, &data[0][0]}; data[0][0] = ax; data[0][1] = ay; data[0][2] = az; @@ -91,7 +92,6 @@ public: data[2][0] = cx; data[2][1] = cy; data[2][2] = cz; - arm_mat = {3, 3, &data[0][0]}; } /** @@ -124,6 +124,14 @@ public: return data[row][col]; } + /** + * set to value + */ + const Matrix3 &operator =(const Matrix3 &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + /** * test for equality */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index adb2293e1..cd00058b5 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -49,7 +49,7 @@ namespace math { template -class Vector { +class VectorBase { public: float data[N]; arm_matrix_instance_f32 arm_col; @@ -57,15 +57,22 @@ public: /** * trivial ctor */ - Vector() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** * setting ctor */ - Vector(const float *d) { - memcpy(data, d, sizeof(data)); +// VectorBase(const float *d) { + // memcpy(data, d, sizeof(data)); + //arm_col = {N, 1, &data[0]}; + //} + + /** + * setting ctor + */ + VectorBase(const float d[]) : data(d) { arm_col = {N, 1, &data[0]}; } @@ -86,7 +93,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -96,7 +103,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -106,7 +113,7 @@ public: /** * set to value */ - const Vector &operator =(const Vector &v) { + const VectorBase &operator =(const VectorBase &v) { memcpy(data, v.data, sizeof(data)); return *this; } @@ -114,8 +121,8 @@ public: /** * negation */ - const Vector operator -(void) const { - Vector res; + const VectorBase operator -(void) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = -data[i]; return res; @@ -124,8 +131,8 @@ public: /** * addition */ - const Vector operator +(const Vector &v) const { - Vector res; + const VectorBase operator +(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] + v(i); return res; @@ -134,8 +141,8 @@ public: /** * subtraction */ - const Vector operator -(const Vector &v) const { - Vector res; + const VectorBase operator -(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] - v(i); return res; @@ -144,23 +151,23 @@ public: /** * uniform scaling */ - const Vector operator *(const float num) const { - Vector temp(*this); + const VectorBase operator *(const float num) const { + VectorBase temp(*this); return temp *= num; } /** * uniform scaling */ - const Vector operator /(const float num) const { - Vector temp(*this); + const VectorBase operator /(const float num) const { + VectorBase temp(*this); return temp /= num; } /** * addition */ - const Vector &operator +=(const Vector &v) { + const VectorBase &operator +=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); return *this; @@ -169,7 +176,7 @@ public: /** * subtraction */ - const Vector &operator -=(const Vector &v) { + const VectorBase &operator -=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); return *this; @@ -178,7 +185,7 @@ public: /** * uniform scaling */ - const Vector &operator *=(const float num) { + const VectorBase &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; return *this; @@ -187,7 +194,7 @@ public: /** * uniform scaling */ - const Vector &operator /=(const float num) { + const VectorBase &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; return *this; @@ -196,7 +203,7 @@ public: /** * dot product */ - float operator *(const Vector &v) const { + float operator *(const VectorBase &v) const { float res; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); @@ -227,11 +234,48 @@ public: /** * returns the normalized version of this vector */ - Vector normalized() const { + VectorBase normalized() const { return *this / length(); } }; +template +class Vector : public VectorBase { +public: + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class Vector<3> : public VectorBase<3> { +public: + Vector<3>() { + arm_col = {3, 1, &this->data[0]}; + } + + Vector<3>(const float x, const float y, const float z) { + data[0] = x; + data[1] = y; + data[2] = z; + arm_col = {3, 1, &this->data[0]}; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + } #endif // VECTOR_HPP diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 13e6dad51..305ebbefa 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -58,15 +58,31 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix3f m; - m.identity(); - Matrix3f m1; - Matrix<3,3> mq; - mq.identity(); - Matrix<3,3> mq1; - m1(0, 0) = 5.0; - Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); - Vector3f v1; + Matrix<3,3> m3; + m3.identity(); + Matrix<4,4> m4; + m4.identity(); + Vector<3> v3; + v3(0) = 1.0f; + v3(1) = 2.0f; + v3(2) = 3.0f; + Vector<4> v4; + v4(0) = 1.0f; + v4(1) = 2.0f; + v4(2) = 3.0f; + v4(3) = 4.0f; + Vector<3> vres3; + Matrix<3,3> mres3; + Matrix<4,4> mres4; + + Matrix3f m3old; + m3old.identity(); + Vector3f v3old; + v3old.x = 1.0f; + v3old.y = 2.0f; + v3old.z = 3.0f; + Vector3f vres3old; + Matrix3f mres3old; unsigned int n = 60000; @@ -74,41 +90,51 @@ int test_mathlib(int argc, char *argv[]) t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - v1 = m * v; + vres3 = m3 * v3; } t1 = hrt_absolute_time(); - warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - mq1 = mq * mq; + vres3old = m3old * v3old; } t1 = hrt_absolute_time(); - warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); - mq1.dump(); + warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.transposed(); + mres3 = m3 * m3; } t1 = hrt_absolute_time(); - warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.inversed(); + mres3old = m3old * m3old; } t1 = hrt_absolute_time(); - warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); - - Matrix<4,4> mn; - mn(0, 0) = 2.0f; - mn(1, 0) = 3.0f; - for (int i = 0; i < mn.getRows(); i++) { - for (int j = 0; j < mn.getCols(); j++) { - printf("%.3f ", mn(i, j)); - } - printf("\n"); + warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres4 = m4 * m4; } + t1 = hrt_absolute_time(); + warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } -- cgit v1.2.3 From ba612c3ee8b88b9352e7cfa723997887dd736b76 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 14:10:25 +0400 Subject: mathlib fixes --- src/lib/conversion/rotation.cpp | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 ++- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Matrix.hpp | 172 +++++++--- src/lib/mathlib/math/Matrix3.cpp | 46 --- src/lib/mathlib/math/Matrix3.hpp | 356 --------------------- src/lib/mathlib/math/Quaternion.hpp | 79 ++++- src/lib/mathlib/math/Vector.hpp | 224 ++++++++++--- src/lib/mathlib/math/Vector2.hpp | 269 ---------------- src/lib/mathlib/math/Vector3.hpp | 287 ----------------- src/lib/mathlib/mathlib.h | 3 - src/lib/mathlib/module.mk | 1 - src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 92 +++--- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../commander/accelerometer_calibration.cpp | 14 +- src/modules/mavlink/mavlink_receiver.cpp | 11 +- src/modules/navigator/navigator_main.cpp | 16 +- src/modules/sensors/sensors.cpp | 14 +- src/systemcmds/tests/test_mathlib.cpp | 23 -- 22 files changed, 510 insertions(+), 1188 deletions(-) delete mode 100644 src/lib/mathlib/math/Matrix3.cpp delete mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector3.hpp (limited to 'src/systemcmds/tests') diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index bac594ab9..614877b18 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) +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; diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fc..0c56494c5 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959..fb7ed486d 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a92..5c0804a39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2..d7f350e5c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -29,7 +29,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -257,7 +257,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index f8f832ed7..68832a039 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -338,7 +338,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1849f58be..67214133a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -42,16 +42,19 @@ #ifndef MATRIX_HPP #define MATRIX_HPP +#include #include "../CMSIS/Include/arm_math.h" namespace math { -template +template class Matrix; +class Quaternion; + // MxN matrix with float elements -template +template class MatrixBase { public: /** @@ -69,10 +72,14 @@ public: * note that this ctor will not initialize elements */ MatrixBase() { - //arm_mat_init_f32(&arm_mat, M, N, data); arm_mat = {M, N, &data[0][0]}; } + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + /** * access by index */ @@ -98,10 +105,10 @@ public: /** * test for equality */ - bool operator ==(const MatrixBase &m) { + bool operator ==(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return false; return true; } @@ -109,10 +116,10 @@ public: /** * test for inequality */ - bool operator !=(const MatrixBase &m) { + bool operator !=(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return true; return false; } @@ -120,85 +127,97 @@ public: /** * set to value */ - const MatrixBase &operator =(const MatrixBase &m) { + const Matrix &operator =(const Matrix &m) { memcpy(data, m.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - MatrixBase operator -(void) const { - MatrixBase res; + Matrix operator -(void) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = -data[i][j]; + res.data[i][j] = -data[i][j]; return res; } /** * addition */ - MatrixBase operator +(const MatrixBase &m) const { - MatrixBase res; + Matrix operator +(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = data[i][j] + m(i, j); + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } - MatrixBase &operator +=(const MatrixBase &m) { - return *this = *this + m; + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *reinterpret_cast*>(this); } /** * subtraction */ - MatrixBase operator -(const MatrixBase &m) const { - MatrixBase res; + 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[i][j] = data[i][j] - m(i, j); + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } - MatrixBase &operator -=(const MatrixBase &m) { - return *this = *this - m; + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - MatrixBase operator *(const float num) const { - MatrixBase res; + 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[i][j] = data[i][j] * num; + res.data[i][j] = data[i][j] * num; return res; } - MatrixBase &operator *=(const float num) { - return *this = *this * num; + 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 *reinterpret_cast*>(this); } - MatrixBase operator /(const float num) const { - MatrixBase res; + 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[i][j] = data[i][j] / num; return res; } - MatrixBase &operator /=(const float num) { - return *this = *this / num; + 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 *this; } /** * multiplication by another matrix */ template - Matrix operator *(const Matrix &m) const { - Matrix res; + Matrix operator *(const Matrix &m) const { + Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; } @@ -230,7 +249,7 @@ public: data[i][i] = 1; } - void dump(void) { + void print(void) { for (unsigned int i = 0; i < M; i++) { for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); @@ -244,6 +263,12 @@ class Matrix : public MatrixBase { public: using MatrixBase::operator *; + Matrix() : MatrixBase() { + } + + Matrix(const float *d) : MatrixBase(d) { + } + /** * set to value */ @@ -255,18 +280,28 @@ public: /** * multiplication by a vector */ - Vector operator *(const Vector &v) const { + Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } }; +} + +#include "Quaternion.hpp" +namespace math { template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; + Matrix() : MatrixBase<3, 3>() { + } + + Matrix(const float *d) : MatrixBase<3, 3>(d) { + } + /** * set to value */ @@ -279,9 +314,70 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - return Vector<3>(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]); + 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 quaternion + */ + void from_quaternion(const Quaternion &q) { + float aSq = q.data[0] * q.data[0]; + float bSq = q.data[1] * q.data[1]; + float cSq = q.data[2] * q.data[2]; + float dSq = q.data[3] * q.data[3]; + data[0][0] = aSq + bSq - cSq - dSq; + data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); + data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); + data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); + data[1][1] = aSq - bSq + cSq - dSq; + data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); + data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); + data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); + data[2][2] = aSq - bSq - cSq + dSq; + } + + /** + * 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; + } + + Vector<3> to_euler(void) 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]); + } } }; diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp deleted file mode 100644 index 3f1259dc4..000000000 --- a/src/lib/mathlib/math/Matrix3.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.cpp - * - * 3x3 Matrix - */ - -#include "Matrix3.hpp" - -namespace math -{ -} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp deleted file mode 100644 index c7eb67ba7..000000000 --- a/src/lib/mathlib/math/Matrix3.hpp +++ /dev/null @@ -1,356 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.hpp - * - * 3x3 Matrix - */ - -#ifndef MATRIX3_HPP -#define MATRIX3_HPP - -#include "Vector3.hpp" -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - /** - * matrix data[row][col] - */ - T data[3][3]; - - /** - * struct for using arm_math functions - */ - arm_matrix_instance_f32 arm_mat; - - /** - * trivial ctor - * note that this ctor will not initialize elements - */ - Matrix3() { - arm_mat = {3, 3, &data[0][0]}; - } - - /** - * setting ctor - */ - Matrix3(const T d[3][3]) { - arm_mat = {3, 3, &data[0][0]}; - memcpy(data, d, sizeof(data)); - } - - /** - * setting ctor - */ - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = ax; - data[0][1] = ay; - data[0][2] = az; - data[1][0] = bx; - data[1][1] = by; - data[1][2] = bz; - data[2][0] = cx; - data[2][1] = cy; - data[2][2] = cz; - } - - /** - * casting from a vector3f to a matrix is the tilde operator - */ - Matrix3(const Vector3 &v) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = 0; - data[0][1] = -v.z; - data[0][2] = v.y; - data[1][0] = v.z; - data[1][1] = 0; - data[1][2] = -v.x; - data[2][0] = -v.y; - data[2][1] = v.x; - data[2][2] = 0; - } - - /** - * access by index - */ - inline T &operator ()(unsigned int row, unsigned int col) { - return data[row][col]; - } - - /** - * access to elements by index - */ - inline const T &operator ()(unsigned int row, unsigned int col) const { - return data[row][col]; - } - - /** - * set to value - */ - const Matrix3 &operator =(const Matrix3 &m) { - memcpy(data, m.data, sizeof(data)); - return *this; - } - - /** - * test for equality - */ - bool operator ==(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return false; - return true; - } - - /** - * test for inequality - */ - bool operator !=(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return true; - return false; - } - - /** - * negation - */ - Matrix3 operator -(void) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = -data[i][j]; - return res; - } - - /** - * addition - */ - Matrix3 operator +(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] + m(i, j); - return res; - } - - Matrix3 &operator +=(const Matrix3 &m) { - return *this = *this + m; - } - - /** - * subtraction - */ - Matrix3 operator -(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] - m(i, j); - return res; - } - - Matrix3 &operator -=(const Matrix3 &m) { - return *this = *this - m; - } - - /** - * uniform scaling - */ - Matrix3 operator *(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] * num; - return res; - } - - Matrix3 &operator *=(const T num) { - return *this = *this * num; - } - - Matrix3 operator /(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] / num; - return res; - } - - Matrix3 &operator /=(const T num) { - return *this = *this / num; - } - - /** - * multiplication by a vector - */ - Vector3 operator *(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, - data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, - data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); - } - - /** - * multiplication of transpose by a vector - */ - Vector3 mul_transpose(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, - data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, - data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); - } - - /** - * multiplication by another matrix - */ - Matrix3 operator *(const Matrix3 &m) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) - Matrix3 res; - arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), - data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), - data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), - data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), - data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), - data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), - data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), - data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), - data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); -#endif - } - - Matrix3 &operator *=(const Matrix3 &m) { - return *this = *this * m; - } - - /** - * transpose the matrix - */ - Matrix3 transposed(void) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float - Matrix3 res; - arm_mat_trans_f32(&arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0], data[1][0], data[2][0], - data[0][1], data[1][1], data[2][1], - data[0][2], data[1][2], data[2][2]); -#endif - } - - /** - * inverse the matrix - */ - Matrix3 inversed(void) const { - Matrix3 res; - arm_mat_inverse_f32(&arm_mat, &res.arm_mat); - return res; - } - - /** - * zero the matrix - */ - void zero(void) { - memset(data, 0, sizeof(data)); - } - - /** - * setup the identity matrix - */ - void identity(void) { - memset(data, 0, sizeof(data)); - data[0][0] = 1; - data[1][1] = 1; - data[2][2] = 1; - } - - /** - * check if any elements are NAN - */ - bool is_nan(void) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (isnan(data[i][j])) - return true; - return false; - } - - /** - * create a rotation matrix from given euler angles - * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf - */ - void from_euler(T roll, T pitch, T yaw) { - T cp = (T)cosf(pitch); - T sp = (T)sinf(pitch); - T sr = (T)sinf(roll); - T cr = (T)cosf(roll); - T sy = (T)sinf(yaw); - T cy = (T)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; - } - - // create eulers from a rotation matrix - //void to_euler(float *roll, float *pitch, float *yaw); - - // apply an additional rotation from a body frame gyro vector - // to a rotation matrix. - //void rotate(const Vector3 &g); -}; - -typedef Matrix3 Matrix3f; -} - -#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 64b980ad8..1b0cb3c41 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,15 +44,17 @@ #include #include "../CMSIS/Include/arm_math.h" +#include "Vector.hpp" +#include "Matrix.hpp" namespace math { -class Quaternion { -public: - float real; - Vector3 imag; +template +class Matrix; +class Quaternion : public Vector<4> { +public: /** * trivial ctor */ @@ -62,7 +64,74 @@ public: /** * setting ctor */ - Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { + } + + Quaternion(const Vector<4> &v) : Vector(v) { + } + + Quaternion(const float *v) : Vector(v) { + } + + /** + * access to elements by index + */ + /* + inline float &operator ()(unsigned int i) { + return *(&a + i); + } + */ + + /** + * access to elements by index + */ + /* + inline const float &operator ()(unsigned int i) const { + return *(&a + i); + } + */ + + /** + * addition + */ + /* + const Quaternion operator +(const Quaternion &q) const { + return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); + } + */ + + /** + * subtraction + */ + /* + const Quaternion operator -(const Quaternion &q) const { + return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); + } + */ + + 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; + } + + 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); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index cd00058b5..2df87c74b 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -42,38 +42,50 @@ #ifndef VECTOR_HPP #define VECTOR_HPP +#include #include #include "../CMSIS/Include/arm_math.h" namespace math { +template +class Vector; + template class VectorBase { public: + /** + * vector data + */ float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ arm_matrix_instance_f32 arm_col; /** * trivial ctor */ - VectorBase() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** - * setting ctor + * copy ctor */ -// VectorBase(const float *d) { - // memcpy(data, d, sizeof(data)); - //arm_col = {N, 1, &data[0]}; - //} + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } /** * setting ctor */ - VectorBase(const float d[]) : data(d) { + VectorBase(const float *d) { arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); } /** @@ -90,10 +102,18 @@ public: return data[i]; } + unsigned int getRows() { + return N; + } + + unsigned int getCols() { + return 1; + } + /** * test for equality */ - bool operator ==(const VectorBase &v) { + bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -103,7 +123,7 @@ public: /** * test for inequality */ - bool operator !=(const VectorBase &v) { + bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -113,98 +133,98 @@ public: /** * set to value */ - const VectorBase &operator =(const VectorBase &v) { + const Vector &operator =(const Vector &v) { memcpy(data, v.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - const VectorBase operator -(void) const { - VectorBase res; + const Vector operator -(void) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = -data[i]; + res.data[i] = -data[i]; return res; } /** * addition */ - const VectorBase operator +(const VectorBase &v) const { - VectorBase res; + const Vector operator +(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] + v(i); + res.data[i] = data[i] + v(i); return res; } /** * subtraction */ - const VectorBase operator -(const VectorBase &v) const { - VectorBase res; + const Vector operator -(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] - v(i); + res.data[i] = data[i] - v(i); return res; } /** * uniform scaling */ - const VectorBase operator *(const float num) const { - VectorBase temp(*this); + const Vector operator *(const float num) const { + Vector temp(*this); return temp *= num; } /** * uniform scaling */ - const VectorBase operator /(const float num) const { - VectorBase temp(*this); + const Vector operator /(const float num) const { + Vector temp(*reinterpret_cast*>(this)); return temp /= num; } /** * addition */ - const VectorBase &operator +=(const VectorBase &v) { + const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *this; + return *reinterpret_cast*>(this); } /** * subtraction */ - const VectorBase &operator -=(const VectorBase &v) { + const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator *=(const float num) { + const Vector &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator /=(const float num) { + const Vector &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *this; + return *reinterpret_cast*>(this); } /** * dot product */ - float operator *(const VectorBase &v) const { - float res; + float operator *(const Vector &v) const { + float res = 0.0f; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); return res; @@ -221,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *this); + return sqrtf(*this * *reinterpret_cast*>(this)); } /** @@ -234,46 +254,174 @@ public: /** * returns the normalized version of this vector */ - VectorBase normalized() const { + Vector normalized() const { return *this / length(); } + + void print(void) { + printf("[ "); + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + printf("]\n"); + } }; template class Vector : public VectorBase { public: + using VectorBase::operator *; + + Vector() : VectorBase() { + } + + Vector(const float d[]) : VectorBase(d) { + } + + Vector(const Vector &v) : VectorBase(v) { + } + + Vector(const VectorBase &v) : VectorBase(v) { + } + /** * set to value */ const Vector &operator =(const Vector &v) { + this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } }; +template <> +class Vector<2> : public VectorBase<2> { +public: + Vector() : VectorBase<2>() { + } + + Vector(const float x, const float y) : VectorBase() { + data[0] = x; + data[1] = y; + } + + Vector(const Vector<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const VectorBase<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float cross(const Vector<2> &b) const { + return data[0]*b.data[1] - data[1]*b.data[0]; + } + + float operator %(const Vector<2> &v) const { + return cross(v); + } + +}; + template <> class Vector<3> : public VectorBase<3> { public: - Vector<3>() { + Vector() { arm_col = {3, 1, &this->data[0]}; } - Vector<3>(const float x, const float y, const float z) { + Vector(const float x, const float y, const float z) { + arm_col = {3, 1, &this->data[0]}; data[0] = x; data[1] = y; data[2] = z; + } + + Vector(const Vector<3> &v) : VectorBase<3>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + } + + /** + * setting ctor + */ + Vector(const float d[]) { arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + +template <> +class Vector<4> : public VectorBase<4> { +public: + Vector() : VectorBase() {} + + Vector(const float x, const float y, const float z, const float t) : VectorBase() { + data[0] = x; + data[1] = y; + data[2] = z; + data[3] = t; } + Vector(const Vector<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + Vector(const VectorBase<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + /** + * setting ctor + */ + /* + Vector(const float d[]) { + arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } +*/ /** * set to value */ + /* const Vector<3> &operator =(const Vector<3> &v) { data[0] = v.data[0]; data[1] = v.data[1]; data[2] = v.data[2]; return *this; } + */ }; } diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp deleted file mode 100644 index 501740614..000000000 --- a/src/lib/mathlib/math/Vector2.hpp +++ /dev/null @@ -1,269 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector2.hpp - * - * 2D Vector - */ - -#ifndef VECTOR2_HPP -#define VECTOR2_HPP - -#include - -namespace math -{ - -template -class Vector2 { -public: - T x, y; - - /** - * trivial ctor - */ - Vector2() { - } - - /** - * setting ctor - */ - Vector2(const T x0, const T y0): x(x0), y(y0) { - } - - /** - * setter - */ - void set(const T x0, const T y0) { - x = x0; - y = y0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector2 &v) { - return (x == v.x && y == v.y); - } - - /** - * test for inequality - */ - bool operator !=(const Vector2 &v) { - return (x != v.x || y != v.y); - } - - /** - * set to value - */ - const Vector2 &operator =(const Vector2 &v) { - x = v.x; - y = v.y; - return *this; - } - - /** - * negation - */ - const Vector2 operator -(void) const { - return Vector2(-x, -y); - } - - /** - * addition - */ - const Vector2 operator +(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); - } - - /** - * subtraction - */ - const Vector2 operator -(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); - } - - /** - * uniform scaling - */ - const Vector2 operator *(const T num) const { - Vector2 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector2 operator /(const T num) const { - Vector2 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector2 &operator +=(const Vector2 &v) { - x += v.x; - y += v.y; - return *this; - } - - /** - * subtraction - */ - const Vector2 &operator -=(const Vector2 &v) { - x -= v.x; - y -= v.y; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator *=(const T num) { - x *= num; - y *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator /=(const T num) { - x /= num; - y /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector2 &v) const { - return x * v.x + y * v.y; - } - - /** - * cross product - */ - const float operator %(const Vector2 &v) const { - return x * v.y - y * v.x; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector2 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector2 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector2 projected(const Vector2 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector2 &v1, const Vector2 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector2 Vector2f; -} - -#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index a0c3b9290..000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector3.hpp - * - * 3D Vector - */ - -#ifndef VECTOR3_HPP -#define VECTOR3_HPP - -#include -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -template -class Vector3 { -public: - T x, y, z; - arm_matrix_instance_f32 arm_col; - - /** - * trivial ctor - */ - Vector3() { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { - arm_col = {3, 1, &x}; - } - - /** - * setter - */ - void set(const T x0, const T y0, const T z0) { - x = x0; - y = y0; - z = z0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector3 &v) { - return (x == v.x && y == v.y && z == v.z); - } - - /** - * test for inequality - */ - bool operator !=(const Vector3 &v) { - return (x != v.x || y != v.y || z != v.z); - } - - /** - * set to value - */ - const Vector3 &operator =(const Vector3 &v) { - x = v.x; - y = v.y; - z = v.z; - return *this; - } - - /** - * negation - */ - const Vector3 operator -(void) const { - return Vector3(-x, -y, -z); - } - - /** - * addition - */ - const Vector3 operator +(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); - } - - /** - * subtraction - */ - const Vector3 operator -(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); - } - - /** - * uniform scaling - */ - const Vector3 operator *(const T num) const { - Vector3 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector3 operator /(const T num) const { - Vector3 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector3 &operator +=(const Vector3 &v) { - x += v.x; - y += v.y; - z += v.z; - return *this; - } - - /** - * subtraction - */ - const Vector3 &operator -=(const Vector3 &v) { - x -= v.x; - y -= v.y; - z -= v.z; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator *=(const T num) { - x *= num; - y *= num; - z *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator /=(const T num) { - x /= num; - y /= num; - z /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector3 &v) const { - return x * v.x + y * v.y + z * v.z; - } - - /** - * cross product - */ - const Vector3 operator %(const Vector3 &v) const { - Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - return temp; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector3 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector3 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector3 projected(const Vector3 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector3 &v1, const Vector3 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector3 Vector3f; -} - -#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index d8e95b46b..9e03855c5 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -42,10 +42,7 @@ #pragma once #include "math/Vector.hpp" -#include "math/Vector2.hpp" -#include "math/Vector3.hpp" #include "math/Matrix.hpp" -#include "math/Matrix3.hpp" #include "math/Quaternion.hpp" #include "math/Limits.hpp" diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 50951c617..191e2da73 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,7 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Matrix3.cpp \ math/Limits.cpp # diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7..6533eb7cf 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), + F(), + G(), + P(), + P0(), + V(), // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), + HAtt(), + RAtt(), // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), + HPos(), + RPos(), // attitude representations C_nb(), q(), @@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb.from_quaternion(q); // HPos is constant HPos(0, 3) = 1.0f; @@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb.from_quaternion(q); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +578,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +594,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +613,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,9 +630,9 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -669,7 +667,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +676,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +686,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,9 +696,9 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -735,7 +733,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a6..46ee4b6c8 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a..36b75dd58 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix board_rotation(3, 3); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix board_rotation_t = board_rotation.transpose(); - math::Vector3 accel_offs_vec; - accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix accel_T_mat(3, 3); - accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..653d4b6b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb; + C_nb.from_quaternion(q); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444a..04307d96b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -422,11 +422,11 @@ Navigator::task_main() mission_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); // Global position - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* AUTONOMOUS FLIGHT */ @@ -436,19 +436,19 @@ Navigator::task_main() if (_mission_valid) { // Next waypoint - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); + prev_wp(0) = _global_triplet.previous.lat / 1e7f; + prev_wp(1) = _global_triplet.previous.lon / 1e7f; } else { /* * No valid next waypoint, go for heading hold. * This is automatically handled by the L1 library. */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + prev_wp(0) = _global_triplet.current.lat / 1e7f; + prev_wp(1) = _global_triplet.current.lon / 1e7f; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec..5baab4a5d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -211,8 +211,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ struct { @@ -457,8 +457,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), + _board_rotation(), + _external_mag_rotation(), _mag_is_external(false) { @@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 305ebbefa..d2e1a93e3 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -75,15 +75,6 @@ int test_mathlib(int argc, char *argv[]) Matrix<3,3> mres3; Matrix<4,4> mres4; - Matrix3f m3old; - m3old.identity(); - Vector3f v3old; - v3old.x = 1.0f; - v3old.y = 2.0f; - v3old.z = 3.0f; - Vector3f vres3old; - Matrix3f mres3old; - unsigned int n = 60000; hrt_abstime t0, t1; @@ -95,13 +86,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3old = m3old * v3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres3 = m3 * m3; @@ -109,13 +93,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3old = m3old * m3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres4 = m4 * m4; -- cgit v1.2.3 From 9dfe366e908ce0100875996c3ea0d4cfdfcc24bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 24 Dec 2013 23:56:28 +0400 Subject: mathlib: Vector class major cleanup --- src/lib/mathlib/math/Matrix.hpp | 27 +--- src/lib/mathlib/math/Quaternion.hpp | 48 +++++- src/lib/mathlib/math/Vector.hpp | 195 +++++++++++------------- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 8 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sensors/sensors.cpp | 6 +- src/systemcmds/tests/test_mathlib.cpp | 114 ++++++++------ 7 files changed, 209 insertions(+), 192 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 7ed8879a7..9896a16d0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -94,11 +94,11 @@ public: return data[row][col]; } - unsigned int getRows() { + unsigned int get_rows() { return M; } - unsigned int getCols() { + unsigned int get_cols() { return N; } @@ -295,11 +295,7 @@ public: return res; } }; -} - -#include "Quaternion.hpp" -namespace math { template <> class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: @@ -329,25 +325,6 @@ public: return res; } - /** - * create a rotation matrix from given quaternion - */ - void from_quaternion(const Quaternion &q) { - float aSq = q.data[0] * q.data[0]; - float bSq = q.data[1] * q.data[1]; - float cSq = q.data[2] * q.data[2]; - float dSq = q.data[3] * q.data[3]; - data[0][0] = aSq + bSq - cSq - dSq; - data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); - data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); - data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); - data[1][1] = aSq - bSq + cSq - dSq; - data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); - data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); - data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); - data[2][2] = aSq - bSq - cSq + dSq; - } - /** * create a rotation matrix from given euler angles * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 3735fb3d3..c19dbd29c 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -58,7 +58,7 @@ public: /** * trivial ctor */ - Quaternion() { + Quaternion() : Vector() { } /** @@ -70,10 +70,29 @@ public: Quaternion(const Vector<4> &v) : Vector(v) { } - Quaternion(const float *v) : Vector(v) { + Quaternion(const Quaternion &q) : Vector(q) { } - Quaternion derivative(const Vector<3> &w) { + Quaternion(const float v[4]) : Vector(v) { + } + + 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]); + } + + /** + * 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], @@ -85,6 +104,9 @@ public: return Q * v * 0.5f; } + /** + * 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); @@ -97,6 +119,26 @@ public: data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) 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; + } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 744402e21..d579ecf73 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * 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 @@ -36,13 +37,12 @@ /** * @file Vector.hpp * - * Generic Vector + * Vector class */ #ifndef VECTOR_HPP #define VECTOR_HPP -#include #include #include #include "../CMSIS/Include/arm_math.h" @@ -84,7 +84,7 @@ public: /** * setting ctor */ - VectorBase(const float *d) { + VectorBase(const float d[N]) { arm_col = {N, 1, &data[0]}; memcpy(data, d, sizeof(data)); } @@ -92,31 +92,30 @@ public: /** * access to elements by index */ - inline float &operator ()(unsigned int i) { + float &operator ()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - inline const float &operator ()(unsigned int i) const { + float operator ()(const unsigned int i) const { return data[i]; } - unsigned int getRows() { + /** + * get rows number + */ + unsigned int get_size() const { return N; } - unsigned int getCols() { - return 1; - } - /** * test for equality */ bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return false; return true; } @@ -126,7 +125,7 @@ public: */ bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return true; return false; } @@ -155,7 +154,7 @@ public: const Vector operator +(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v(i); + res.data[i] = data[i] + v.data[i]; return res; } @@ -165,7 +164,7 @@ public: const Vector operator -(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v(i); + res.data[i] = data[i] - v.data[i]; return res; } @@ -173,16 +172,20 @@ public: * uniform scaling */ const Vector operator *(const float num) const { - Vector temp(*this); - return temp *= num; + 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 temp(*static_cast*>(this)); - return temp /= num; + Vector res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } /** @@ -190,7 +193,7 @@ public: */ const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] += v(i); + data[i] += v.data[i]; return *static_cast*>(this); } @@ -199,7 +202,7 @@ public: */ const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] -= v(i); + data[i] -= v.data[i]; return *static_cast*>(this); } @@ -227,7 +230,7 @@ public: float operator *(const Vector &v) const { float res = 0.0f; for (unsigned int i = 0; i < N; i++) - res += data[i] * v(i); + res += data[i] * v.data[i]; return res; } @@ -235,14 +238,20 @@ public: * gets the length of this vector squared */ float length_squared() const { - return (*this * *this); + 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 { - return sqrtf(*this * *static_cast*>(this)); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return sqrtf(res); } /** @@ -277,25 +286,17 @@ public: template class __EXPORT Vector : public VectorBase { public: - using VectorBase::operator *; + //using VectorBase::operator *; + Vector() : VectorBase() {} - Vector() : VectorBase() { - } + Vector(const Vector &v) : VectorBase(v) {} - Vector(const float d[]) : VectorBase(d) { - } - - Vector(const Vector &v) : VectorBase(v) { - } - - Vector(const VectorBase &v) : VectorBase(v) { - } + Vector(const float d[N]) : VectorBase(d) {} /** * set to value */ const Vector &operator =(const Vector &v) { - this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } @@ -304,22 +305,22 @@ public: template <> class __EXPORT Vector<2> : public VectorBase<2> { public: - Vector() : VectorBase<2>() { - } + Vector() : VectorBase<2>() {} - Vector(const float x, const float y) : VectorBase() { - data[0] = x; - data[1] = y; - } - - Vector(const Vector<2> &v) : VectorBase() { + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<2>() { data[0] = v.data[0]; data[1] = v.data[1]; } - Vector(const VectorBase<2> &v) : VectorBase() { - 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; } /** @@ -331,55 +332,49 @@ public: return *this; } - float cross(const Vector<2> &b) const { - return data[0]*b.data[1] - data[1]*b.data[0]; - } - float operator %(const Vector<2> &v) const { - return cross(v); + return data[0] * v.data[1] - data[1] * v.data[0]; } - }; template <> class __EXPORT Vector<3> : public VectorBase<3> { public: - Vector() { - arm_col = {3, 1, &this->data[0]}; - } + Vector() : VectorBase<3>() {} - Vector(const float x, const float y, const float z) { - arm_col = {3, 1, &this->data[0]}; - data[0] = x; - data[1] = y; - data[2] = z; + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; } - Vector(const Vector<3> &v) : VectorBase<3>() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; } - /** - * setting ctor - */ - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; } /** * set to value */ const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + 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 <> @@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const float x, const float y, const float z, const float t) : VectorBase() { - data[0] = x; - data[1] = y; - data[2] = z; - data[3] = t; + Vector(const Vector &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; } - Vector(const Vector<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; } - Vector(const VectorBase<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + 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; } - /** - * setting ctor - */ - /* - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; - } -*/ /** * set to value */ - /* - const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; return *this; } - */ }; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 9d3ef07f2..aca3fe7b6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt) } // C_nb update - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // euler update Vector<3> euler = C_nb.to_euler(); @@ -628,7 +628,7 @@ int KalmanNav::correctAtt() Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { @@ -694,7 +694,7 @@ int KalmanNav::correctPos() Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 653d4b6b3..b4f7f2dfe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb; - C_nb.from_quaternion(q); + math::Matrix<3,3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eea9438f7..9e2eeafa4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -465,8 +465,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(), - _external_mag_rotation(), _mag_is_external(false) { @@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index d2e1a93e3..e654e0f81 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -48,6 +48,8 @@ #include "tests.h" +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + using namespace math; const char* formatResult(bool res) { @@ -58,60 +60,82 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix<3,3> m3; - m3.identity(); - Matrix<4,4> m4; - m4.identity(); - Vector<3> v3; - v3(0) = 1.0f; - v3(1) = 2.0f; - v3(2) = 3.0f; - Vector<4> v4; - v4(0) = 1.0f; - v4(1) = 2.0f; - v4(2) = 3.0f; - v4(3) = 4.0f; - Vector<3> vres3; - Matrix<3,3> mres3; - Matrix<4,4> mres4; - - unsigned int n = 60000; + Vector<2> v2(1.0f, 2.0f); + Vector<3> v3(1.0f, 2.0f, 3.0f); + Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); + Vector<10> v10; + v10.zero(); - hrt_abstime t0, t1; + float data2[2] = {1.0f, 2.0f}; + float data3[3] = {1.0f, 2.0f, 3.0f}; + float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + float data10[10]; - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3 = m3 * v3; + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + TEST_OP("Constructor Vector<2>()", Vector<2> v); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(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); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3 * m3; + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + TEST_OP("Constructor Vector<3>()", Vector<3> v); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(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("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres4 = m4 * m4; + { + 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); + TEST_OP("Constructor Vector<4>()", Vector<4> v); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(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); } - t1 = hrt_absolute_time(); - warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.transposed(); + { + TEST_OP("Constructor Vector<10>()", Vector<10> v); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); } - t1 = hrt_absolute_time(); - warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.inversed(); - } - t1 = hrt_absolute_time(); - warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } -- cgit v1.2.3 From 8ed193d1159dd64e3bd44668e75aac8b71fa3fa2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Dec 2013 13:03:36 +0400 Subject: mathlib: Matrix and Quaternion cleanup and bugfixes. Copyright updated. --- src/lib/mathlib/math/Matrix.hpp | 57 +++++++++++------- src/lib/mathlib/math/Quaternion.hpp | 40 +++++++------ src/lib/mathlib/math/Vector.hpp | 20 +++---- src/modules/att_pos_estimator_ekf/kalman_main.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 70 ++++++++++++++--------- 5 files changed, 114 insertions(+), 75 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9896a16d0..584e5e81b 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * 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 @@ -36,7 +37,7 @@ /** * @file Matrix.hpp * - * Generic Matrix + * Matrix class */ #ifndef MATRIX_HPP @@ -51,8 +52,6 @@ namespace math template class __EXPORT Matrix; -class __EXPORT Quaternion; - // MxN matrix with float elements template class __EXPORT MatrixBase { @@ -75,6 +74,14 @@ public: arm_mat = {M, N, &data[0][0]}; } + /** + * 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)); @@ -83,29 +90,35 @@ public: /** * access by index */ - inline float &operator ()(unsigned int row, unsigned int col) { + float &operator ()(const unsigned int row, const unsigned int col) { return data[row][col]; } /** * access by index */ - inline const float &operator ()(unsigned int row, unsigned int col) const { + float operator ()(const unsigned int row, const unsigned int col) const { return data[row][col]; } - unsigned int get_rows() { + /** + * get rows number + */ + unsigned int get_rows() const { return M; } - unsigned int get_cols() { + /** + * get columns number + */ + unsigned int get_cols() const { return N; } /** * test for equality */ - bool operator ==(const Matrix &m) { + 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]) @@ -116,7 +129,7 @@ public: /** * test for inequality */ - bool operator !=(const Matrix &m) { + 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]) @@ -209,7 +222,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] /= num; - return *this; + return *static_cast*>(this); } /** @@ -272,11 +285,11 @@ class __EXPORT Matrix : public MatrixBase { public: using MatrixBase::operator *; - Matrix() : MatrixBase() { - } + Matrix() : MatrixBase() {} - Matrix(const float *d) : MatrixBase(d) { - } + Matrix(const Matrix &m) : MatrixBase(m) {} + + Matrix(const float *d) : MatrixBase(d) {} /** * set to value @@ -301,11 +314,11 @@ class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; - Matrix() : MatrixBase<3, 3>() { - } + Matrix() : MatrixBase<3, 3>() {} - Matrix(const float *d) : MatrixBase<3, 3>(d) { - } + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} /** * set to value @@ -348,6 +361,9 @@ public: data[2][2] = cr * cp; } + /** + * get euler angles from rotation matrix + */ Vector<3> to_euler(void) const { Vector<3> euler; euler.data[1] = asinf(-data[2][0]); @@ -364,6 +380,7 @@ public: euler.data[0] = atan2f(data[2][1], data[2][2]); euler.data[2] = atan2f(data[1][0], data[0][0]); } + return euler; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index c19dbd29c..54d4e72ab 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * 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 @@ -36,7 +37,7 @@ /** * @file Quaternion.hpp * - * Quaternion + * Quaternion class */ #ifndef QUATERNION_HPP @@ -50,31 +51,32 @@ namespace math { -template -class Matrix; - -class Quaternion : public Vector<4> { +class __EXPORT Quaternion : public Vector<4> { public: /** * trivial ctor */ - Quaternion() : Vector() { - } + Quaternion() : Vector<4>() {} /** - * setting ctor + * copy ctor */ - Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { - } + Quaternion(const Quaternion &q) : Vector<4>(q) {} - Quaternion(const Vector<4> &v) : Vector(v) { - } + /** + * casting from vector + */ + Quaternion(const Vector<4> &v) : Vector<4>(v) {} - Quaternion(const Quaternion &q) : Vector(q) { - } + /** + * setting ctor + */ + Quaternion(const float d[4]) : Vector<4>(d) {} - Quaternion(const float v[4]) : Vector(v) { - } + /** + * 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 *; @@ -138,8 +140,10 @@ public: 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 index d579ecf73..6bfcc96b6 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -68,6 +68,7 @@ public: /** * trivial ctor + * note that this ctor will not initialize elements */ VectorBase() { arm_col = {N, 1, &data[0]}; @@ -104,7 +105,7 @@ public: } /** - * get rows number + * get vector size */ unsigned int get_size() const { return N; @@ -113,7 +114,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return false; @@ -123,7 +124,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return true; @@ -286,10 +287,9 @@ public: template class __EXPORT Vector : public VectorBase { public: - //using VectorBase::operator *; Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase(v) {} + Vector(const Vector &v) : VectorBase(v) {} Vector(const float d[N]) : VectorBase(d) {} @@ -307,8 +307,8 @@ class __EXPORT Vector<2> : public VectorBase<2> { public: Vector() : VectorBase<2>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : 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]; } @@ -342,8 +342,8 @@ class __EXPORT Vector<3> : public VectorBase<3> { public: Vector() : VectorBase<3>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : 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]; } @@ -382,7 +382,7 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase<4>() { + Vector(const Vector<4> &v) : VectorBase<4>() { for (unsigned int i = 0; i < 4; i++) data[i] = v.data[i]; } diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 70ba628f3..3d20d4d2d 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 8096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index e654e0f81..693a208ba 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -60,25 +60,15 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Vector<2> v2(1.0f, 2.0f); - Vector<3> v3(1.0f, 2.0f, 3.0f); - Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); - Vector<10> v10; - v10.zero(); - - float data2[2] = {1.0f, 2.0f}; - float data3[3] = {1.0f, 2.0f, 3.0f}; - float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; - float data10[10]; - { Vector<2> v; Vector<2> v1(1.0f, 2.0f); Vector<2> v2(1.0f, -1.0f); - TEST_OP("Constructor Vector<2>()", Vector<2> v); - TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); - TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); - TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.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)); + 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); @@ -92,10 +82,11 @@ int test_mathlib(int argc, char *argv[]) Vector<3> v; Vector<3> v1(1.0f, 2.0f, 0.0f); Vector<3> v2(1.0f, -1.0f, 2.0f); - TEST_OP("Constructor Vector<3>()", Vector<3> v); - TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); - TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); - TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.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)); + 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); @@ -119,10 +110,11 @@ int test_mathlib(int argc, char *argv[]) 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); - TEST_OP("Constructor Vector<4>()", Vector<4> v); - TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); - TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); - TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.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)); + 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); @@ -132,9 +124,35 @@ int test_mathlib(int argc, char *argv[]) } { - TEST_OP("Constructor Vector<10>()", Vector<10> v); - TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); - TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); + 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)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> 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); + } + + { + 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); } return 0; -- cgit v1.2.3