fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 06:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ControlMath: remove unused methods

Browse Source
This commit is contained in:
Dennis Mannhart
2018-03-05 10:30:20 +01:00
committed by Beat Küng
parent 81da94ff46
commit 0c6c771620
3 changed files with 0 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
-115
View File
@@ -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)
{