From 62cc25022c0f3871b8872a9f037caa722694a6a9 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sun, 31 Dec 2017 17:52:57 +1030
Subject: [PATCH] AC_AttitudeControl: protect against overshoot in sqrt
 controller

---
 .../AC_AttitudeControl/AC_AttitudeControl.cpp | 44 ++++++++++++-------
 1 file changed, 27 insertions(+), 17 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index d5bea439d5..da2831215a 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -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);
+    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 {
+            correction_rate = 0.0f;
         }
-        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 {
-        return error*p;
+        // 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;
     }
 }