From 0c6c7716204f46195f7d9edeacde3f503cbd957f Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Mon, 5 Mar 2018 10:30:20 +0100
Subject: [PATCH] ControlMath: remove unused methods
---
.../mc_pos_control/Utility/ControlMath.cpp | 115 -----------
.../mc_pos_control/Utility/ControlMath.hpp | 2 -
.../mc_pos_control_tests/test_controlmath.cpp | 194 ------------------
3 files changed, 311 deletions(-)
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
index cfc987002c..9235907b7c 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.cpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -47,121 +47,6 @@
namespace ControlMath
{
-
-/**
- * Limit vector based on a maximum tilt.
- *
- * @param vec: 3d vector in N-E-D frame
- * @param tilt_max: maximum tilt allowed
- * @return 3d vector adjusted to tilt
- *
- * Tilt is adjusted such that vector component in D-direction
- * has higher priority.
- */
-matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float maximum_tilt)
-{
- /* We only consider maximum tilt < 90 */
- float tilt_max = maximum_tilt;
-
- if (tilt_max > M_PI_2_F) {
- tilt_max = M_PI_2_F;
- }
-
- /* Desired tilt is above 90 -> in order to stay within tilt,
- * vector has to be zero (N-E-D frame)*/
- if (vec(2) > 0.0f) {
- return matrix::Vector3f();
- }
-
- /* Maximum tilt is 0 */
- if (tilt_max < 0.001f) {
- return matrix::Vector3f(0.0f, 0.0f, vec(2));
- }
-
- /* desired and maximum allowed horizontal magnitude */
- float xy_mag = matrix::Vector2f(vec(0), vec(1)).length();
- float xy_mag_max = fabsf(vec(2)) * tanf(tilt_max);
-
- if (xy_mag_max < xy_mag) {
- float x0 = vec(0) * xy_mag_max / xy_mag;
- float x1 = vec(1) * xy_mag_max / xy_mag;
- return matrix::Vector3f(x0, x1, vec(2));
- }
-
- /* No adjustment: return normal vec */
- return vec;
-}
-
-/**
- * Constrain output from PID (u-vector) with priority on altitude.
- *
- * @reference param u: PID output in N-E-D frame.
- * @reference param stop_I: boolean for xy and z, indicating when integration for PID
- * should stop: true = stop integration, false = continue integration
- * @Ulimits: Ulimits[0] = Umax, Ulimits[1] = Umin
- * @d: direction given by (r - y ); r = reference, y = measurement
- *
- * Saturation strategy:
- * u >= Umax and d >= 0 => Saturation = true
- * u >= Umax and d <= 0 => Saturation = false
- * u <= Umin and d <= 0 => Saturation = true
- * u <= Umin and d >= 0 => Saturation = false
- *
- *
- */
-
-void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2], const float d[2])
-{
- stop_I[0] = stop_I[1] = false;
- float xy_max = sqrtf(Ulimits[0] * Ulimits[0] - u(2) * u(2));
- float xy_mag = matrix::Vector2f(u(0), u(0)).length();
-
- if (u(2) * u(2) >= Ulimits[0] * Ulimits[0]) {
- /* The desired u in D-direction exceeds maximum */
-
- /* Check if altitude saturated */
- if (d[1] >= 0.0f) {
- stop_I[1] = true;
- }
-
- stop_I[0] = true;
- u(0) = 0.0f;
- u(1) = 0.0f;
- u(2) = math::sign(u(2)) * Ulimits[0];
-
- } else if (u.length() >= Ulimits[0]) {
-
- /* The desired u_xy exceeds maximum */
-
- if (d[0] >= 0.0f) {
- stop_I[0] = true;
- }
-
- u(0) = u(0) / xy_mag * xy_max;
- u(1) = u(1) / xy_mag * xy_max;
-
- } else if (u.length() <= Ulimits[1]) {
- /* The desired u is below minimum */
-
- /* Check if z or xy are saturated */
-
- if (d[1] <= 0.0f) {
- stop_I[1] = true;
- }
-
- /* If we have zero vector,
- * then apply minimum throttle in D-direction
- * since we do not know better. (no direction given)
- */
- if (u.length() < 0.0001f) {
- u = matrix::Vector3f(0.0f, 0.0f, -Ulimits[1]);
-
- } else {
- u = u.normalized() * Ulimits[1];
- }
- }
-}
-
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp)
{
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.hpp b/src/modules/mc_pos_control/Utility/ControlMath.hpp
index ca0058e8bf..c82b5285d0 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.hpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.hpp
@@ -46,7 +46,5 @@
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]);
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
}
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 2facb40fcf..9c092b14d8 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
@@ -10,210 +10,16 @@ public:
virtual bool run_tests();
private:
- bool testConstrainTilt();
- bool testConstrainPIDu();
bool testThrAttMapping();
-
-
};
bool ControlMathTest::run_tests()
{
- ut_run_test(testConstrainTilt);
- ut_run_test(testConstrainPIDu);
ut_run_test(testThrAttMapping);
return (_tests_failed == 0);
}
-bool ControlMathTest::testConstrainTilt()
-{
- // expected: return same vector
- // reason: tilt exceeds maximum tilt
- matrix::Vector3f v(0.5f, 0.5f, 0.1f);
- float tilt_max = math::radians(91.0f);
- matrix::Vector3f vr = ControlMath::constrainTilt(v, tilt_max);
- ut_assert_true((v - vr).length() < EPS);
-
- // expected: return zero vector
- // reason: v points down, but cone generated by tilt is only
- // defined in negative z (upward).
- v = matrix::Vector3f(1.0f, 1.0f, 0.1f);
- tilt_max = math::radians(45.0f);
- 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 = ControlMath::constrainTilt(v, tilt_max);
- float vr_xy = matrix::Vector2f(vr(0), vr(1)).length();
- ut_assert_true(fabsf(vr(2)) - vr_xy < EPS);
-
- // expected: length vr_z larger than vr_xy
- // 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 = ControlMath::constrainTilt(v, tilt_max);
- vr_xy = matrix::Vector2f(vr(0), vr(1)).length();
- ut_assert_true(fabsf(vr(2)) - vr_xy > EPS);
-
- // expected: length of vr_xy larger than vr_z
- // 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 = ControlMath::constrainTilt(v, tilt_max);
- vr_xy = matrix::Vector2f(vr(0), vr(1)).length();
- ut_assert_true(fabsf(vr(2)) - vr_xy < EPS);
-
- // expected: same vector is return
- // reson: vector is within cond
- v = matrix::Vector3f(1.0f, 1.0f, -0.5f);
- tilt_max = math::radians(89.f);
- vr = ControlMath::constrainTilt(v, tilt_max);
- ut_assert_true((v - vr).length() < EPS);
-
- return true;
-
-}
-
-bool ControlMathTest::testConstrainPIDu()
-{
- /* Notation:
- * u: input thrust that gets modified
- * u_o: unmodified thrust input
- * sat: saturation flags
- * Ulim: max and min thrust
- * d: flags for xy and z, indicating sign of (r-y)
- * r: reference; not used here
- * y: measurement; not used here
- */
-
- // expected: same u
- // reason: no direction change and within bounds
- bool sat[2] = {false, false};
- float Ulim[2] = {0.8f, 0.2f};
- matrix::Vector3f u{0.1f, 0.1f, -0.4f};
- matrix::Vector3f u_o = u;
- float d[2] = {1.0f, 1.0f};
- ControlMath::constrainPIDu(u, sat, Ulim, d);
- ut_assert_true((u - u_o).length() < EPS);
- ut_assert_false(sat[1]);
- ut_assert_false(sat[0]);
-
- // expected: u_xy smaller than u_o_xy and sat[0] = true
- // reason: u_o_xy exceeds Ulim[0] and d[0] is positive
- sat[0] = false;
- sat[1] = false;
- Ulim[0] = 0.5f;
- Ulim[1] = 0.2f;
- u = matrix::Vector3f(0.4f, 0.4f, -0.1f);
- u_o = u;
- d[0] = 1.0f;
- d[1] = 1.0f;
- 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);
- ut_assert_true(fabsf(u(2)) - fabsf(u_o(2)) < EPS);
- ut_assert_true(sat[0]);
- ut_assert_false(sat[1]);
-
- // expected: u_xy smaller than u_o_xy and sat[0] = false
- // reason: u_o_xy exceeds Ulim[0] and d[0] is negative
- d[0] = -1.0f;
- ut_assert_true(u_xy < u_o_xy);
- ut_assert_true(fabsf(u(2)) - fabsf(u_o(2)) < EPS);
- ut_assert_true(sat[0]);
- ut_assert_false(sat[1]);
-
- // expected: u_xy = 0 and sat[0] = true
- // expected: u_z = -0.6 (maximum) and sat[1] = true
- // reason: u_o_z exceeds maximum and since altitude
- // has higher priority, u_xy will be set to 0. No direction
- // change desired.
- sat[0] = false;
- sat[1] = false;
- Ulim[0] = 0.5f;
- Ulim[1] = 0.2f;
- u = matrix::Vector3f(0.4f, 0.4f, -0.6f);
- u_o = u;
- d[0] = 1.0f;
- d[1] = 1.0f;
- 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);
- ut_assert_true(u_o(2) - (-0.6f) < EPS);
- ut_assert_true(u_xy < EPS);
- ut_assert_true(sat[0]);
- ut_assert_true(sat[1]);
-
- // expected: u_xy = 0 and sat[0] = true because u_z is saturate
- // => altitude priority
- // expected: u_z = -0.6 (maximum) and sat[1] = true
- // reason: u_o_z exceeds maximum and since altitude
- // has higher priority, u_xy will be set to 0. Direction
- // change desired for xy
- d[0] = -1.0f;
- 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);
- ut_assert_true(u_o(2) - (-0.6f) < EPS);
- ut_assert_true(u_xy < EPS);
- ut_assert_true(sat[0]);
- ut_assert_true(sat[1]);
-
- // expected: nothing
- // reason: thottle within bounds
- sat[0] = false;
- sat[1] = false;
- Ulim[0] = 0.7f;
- Ulim[1] = 0.2f;
- u = matrix::Vector3f(0.3f, 0.3f, 0.0f);
- u_o = u;
- d[0] = 1.0f;
- d[1] = 1.0f;
- ControlMath::constrainPIDu(u, sat, Ulim, d);
- ut_assert_true((u - u_o).length() < EPS);
- ut_assert_false(sat[1]);
- ut_assert_false(sat[0]);
-
- // expected: u_xy at minimum, no saturation
- // reason: u_o is below minimum with u_o_z = 0, which
- // means that Ulim[1] is in xy direction.
- // No saturation because no direction change.
- sat[0] = false;
- sat[1] = false;
- Ulim[0] = 0.7f;
- Ulim[1] = 0.2f;
- u = matrix::Vector3f(0.05f, 0.05f, 0.0f);
- u_o = u;
- d[0] = 1.0f;
- d[1] = 1.0f;
- 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);
- ut_assert_false(sat[1]);
- ut_assert_false(sat[0]);
-
- // expected: u_xy at minimum, saturation in z
- // reason: u_o is below minimum with u_o_z = 0, which
- // means that Ulim[1] is in xy direction.
- // Direction change in z.
- d[1] = -1.0f;
- 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]);
- ut_assert_false(sat[0]);
-
- return true;
-}
-
bool ControlMathTest::testThrAttMapping()
{
--
GitLab