Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: protect against overshoot in sqrt controller

This commit is contained in:
Leonard Hall 2017-12-31 17:52:57 +10:30 committed by Randy Mackay
parent 75de0cb4ef
commit 62cc25022c
1 changed files with 27 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -714,25 +714,35 @@ float AC_AttitudeControl::get_althold_lean_angle_max() const
// Proportional controller with piecewise sqrt sections to constrain second derivative // Proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt) float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{ {
if (second_ord_lim < 0.0f || is_zero(second_ord_lim)) { float correction_rate;
return error*p; if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
// second order limit is zero or negative.
correction_rate = error*p;
} else if (is_zero(p)) { } else if (is_zero(p)) {
if (error > 0.0f && !is_zero(dt)) { // P term is zero but we have a second order limit.
return MIN(safe_sqrt(2.0f*error*second_ord_lim), error*dt); if (is_positive(error)) {
} else if (error < 0.0f && !is_zero(dt)) { correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
return MAX(-safe_sqrt(2.0f*error*second_ord_lim), error*dt); } else if (is_negative(error)) {
} correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
return 0.0f;
}
float linear_dist = second_ord_lim/sq(p);
if (error > linear_dist) {
return safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
} else if (error < -linear_dist) {
return -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
} else { } else {
return error*p; correction_rate = 0.0f;
}
} else {
// Both the P and second order limit have been defined.
float linear_dist = second_ord_lim/sq(p);
if (error > linear_dist) {
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
} else if (error < -linear_dist) {
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
} else {
correction_rate = error*p;
}
}
if (!is_zero(dt)) {
// this ensures we do not get small oscillations by over shooting the error correction in the last time step.
return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
} else {
return correction_rate;
} }
} }