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

46
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
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)) {
return error*p;
}else if (is_zero(p)) {
if (error > 0.0f && !is_zero(dt)) {
return MIN(safe_sqrt(2.0f*error*second_ord_lim), error*dt);
} else if (error < 0.0f && !is_zero(dt)) {
return MAX(-safe_sqrt(2.0f*error*second_ord_lim), error*dt);
}
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)));
float correction_rate;
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)) {
// P term is zero but we have a second order limit.
if (is_positive(error)) {
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
} else if (is_negative(error)) {
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
} 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;
}
}