From a26e46bede186c36b475e0b5a9cf3ef758cc1c9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 23 Dec 2013 22:34:11 +0400 Subject: att_pos_estimator_ekf: fixes, mathlib: minor changes --- src/modules/att_pos_estimator_ekf/kalman_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/att_pos_estimator_ekf/kalman_main.cpp') diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162..70ba628f3 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, - 4096, + 8096, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(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/modules/att_pos_estimator_ekf/kalman_main.cpp') 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