From 09bf73945aa3b2d41685dcc563f5ed5548fdc545 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 22 Dec 2017 09:28:14 +0100 Subject: [PATCH] ControlMath: rename namespace to file name --- .../mc_pos_control/TranslationControl.cpp | 4 +-- .../mc_pos_control/Utility/ControlMath.cpp | 2 +- .../mc_pos_control/Utility/ControlMath.hpp | 2 +- .../mc_pos_control_tests/test_controlmath.cpp | 26 +++++++++---------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/mc_pos_control/TranslationControl.cpp b/src/modules/mc_pos_control/TranslationControl.cpp index c02cb13e4a..f0b990764c 100644 --- a/src/modules/mc_pos_control/TranslationControl.cpp +++ b/src/modules/mc_pos_control/TranslationControl.cpp @@ -187,7 +187,7 @@ void TranslationControl::_velocityController(const float &dt) * It is to note that pure manual and rate control will never enter _velocityController method*/ float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F; tilt_max = math::min(tilt_max, M_PI_2_F); - _thr_sp = PosControl::constrainTilt(_thr_sp, tilt_max); + _thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max); /*TODO: Check if it is beneficial to project thrust onto body z axis */ @@ -214,7 +214,7 @@ void TranslationControl::_velocityController(const float &dt) float dot_xy = matrix::Vector2f(&vel_err(0)) * matrix::Vector2f(&_vel_sp(0)); float direction[2] = {dot_xy, -vel_err(2)}; // negative sign because of N-E-D bool stop_I[2] = {false, false}; // stop integration for xy and z - PosControl::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction); + ControlMath::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction); /* throttle is just thrust length */ _throttle = _thr_sp.length(); diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index b2ce959565..e7f2210d80 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -43,7 +43,7 @@ #include "ControlMath.hpp" #include -namespace PosControl +namespace ControlMath { /** diff --git a/src/modules/mc_pos_control/Utility/ControlMath.hpp b/src/modules/mc_pos_control/Utility/ControlMath.hpp index 72153a1a06..9adacf08a8 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.hpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.hpp @@ -43,7 +43,7 @@ #include -namespace PosControl +namespace ControlMath { matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float &tilt_max); void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2], const float d[2]); diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp index 6b78f7c363..d999b4aa42 100644 --- a/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp @@ -30,7 +30,7 @@ bool ControlMathTest::testConstrainTilt() // reason: tilt exceeds maximum tilt matrix::Vector3f v(0.5f, 0.5f, 0.1f); float tilt_max = math::radians(91.0f); - matrix::Vector3f vr = PosControl::constrainTilt(v, tilt_max); + matrix::Vector3f vr = ControlMath::constrainTilt(v, tilt_max); ut_assert_true((v - vr).length() < EPS); // expected: return zero vector @@ -38,14 +38,14 @@ bool ControlMathTest::testConstrainTilt() // defined in negative z (upward). v = matrix::Vector3f(1.0f, 1.0f, 0.1f); tilt_max = math::radians(45.0f); - vr = PosControl::constrainTilt(v, tilt_max); + vr = ControlMath::constrainTilt(v, tilt_max); ut_assert_true((vr).length() < EPS); // expected: length vr_xy same as vr_z // reason: it is a 45 cone and v_xy exceeds v_z v = matrix::Vector3f(1.0f, 1.0f, -0.5f); tilt_max = math::radians(45.0f); - vr = PosControl::constrainTilt(v, tilt_max); + vr = ControlMath::constrainTilt(v, tilt_max); float vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); ut_assert_true(fabsf(vr(2)) - vr_xy < EPS); @@ -53,7 +53,7 @@ bool ControlMathTest::testConstrainTilt() // reason: it is a 30 cone and v_xy exceeds v_z v = matrix::Vector3f(1.0f, 1.0f, -0.5f); tilt_max = math::radians(20.0f); - vr = PosControl::constrainTilt(v, tilt_max); + vr = ControlMath::constrainTilt(v, tilt_max); vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); ut_assert_true(fabsf(vr(2)) - vr_xy > EPS); @@ -61,7 +61,7 @@ bool ControlMathTest::testConstrainTilt() // reason: it is a 80 cone and v_xy exceeds v_z v = matrix::Vector3f(10.0f, 10.0f, -0.5f); tilt_max = math::radians(80.f); - vr = PosControl::constrainTilt(v, tilt_max); + vr = ControlMath::constrainTilt(v, tilt_max); vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); ut_assert_true(fabsf(vr(2)) - vr_xy < EPS); @@ -69,7 +69,7 @@ bool ControlMathTest::testConstrainTilt() // reson: vector is within cond v = matrix::Vector3f(1.0f, 1.0f, -0.5f); tilt_max = math::radians(89.f); - vr = PosControl::constrainTilt(v, tilt_max); + vr = ControlMath::constrainTilt(v, tilt_max); ut_assert_true((v - vr).length() < EPS); return true; @@ -95,7 +95,7 @@ bool ControlMathTest::testConstrainPIDu() matrix::Vector3f u{0.1f, 0.1f, -0.4f}; matrix::Vector3f u_o = u; float d[2] = {1.0f, 1.0f}; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); ut_assert_true((u - u_o).length() < EPS); ut_assert_false(sat[1]); ut_assert_false(sat[0]); @@ -110,7 +110,7 @@ bool ControlMathTest::testConstrainPIDu() u_o = u; d[0] = 1.0f; d[1] = 1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); float u_xy = matrix::Vector2f(u(0), u(1)).length(); float u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); ut_assert_true(u_xy < u_o_xy); @@ -139,7 +139,7 @@ bool ControlMathTest::testConstrainPIDu() u_o = u; d[0] = 1.0f; d[1] = 1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); u_xy = matrix::Vector2f(u(0), u(1)).length(); u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); ut_assert_true(u_xy < u_o_xy); @@ -155,7 +155,7 @@ bool ControlMathTest::testConstrainPIDu() // has higher priority, u_xy will be set to 0. Direction // change desired for xy d[0] = -1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); u_xy = matrix::Vector2f(u(0), u(1)).length(); u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); ut_assert_true(u_xy < u_o_xy); @@ -174,7 +174,7 @@ bool ControlMathTest::testConstrainPIDu() u_o = u; d[0] = 1.0f; d[1] = 1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); ut_assert_true((u - u_o).length() < EPS); ut_assert_false(sat[1]); ut_assert_false(sat[0]); @@ -191,7 +191,7 @@ bool ControlMathTest::testConstrainPIDu() u_o = u; d[0] = 1.0f; d[1] = 1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); u_xy = matrix::Vector2f(u(0), u(1)).length(); ut_assert_true(u_xy - Ulim[1] < EPS); ut_assert_true(fabsf(u(2)) < EPS); @@ -203,7 +203,7 @@ bool ControlMathTest::testConstrainPIDu() // means that Ulim[1] is in xy direction. // Direction change in z. d[1] = -1.0f; - PosControl::constrainPIDu(u, sat, Ulim, d); + ControlMath::constrainPIDu(u, sat, Ulim, d); ut_assert_true(u_xy - Ulim[1] < EPS); ut_assert_true(fabsf(u(2)) < EPS); ut_assert_true(sat[1]);