mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 13:58:53 +08:00
ControlMath: remove unused methods
This commit is contained in:
Dennis Mannhart
committed by
Beat Küng
Beat Küng
parent
81da94ff46
commit
0c6c771620
@@ -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)
|
||||
{
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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()
|
||||
{
|
||||
|
||||
|
||||
Reference in New Issue
Block a user