AC_AttControl: add sqrt_controller

Original code by Jonathan Challinger

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -712,3 +712,21 @@ int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
 
     return throttle_out;
 }
+
+// sqrt_controller - response based on the sqrt of the error instead of the more common linear response
+float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
+{
+    if (second_ord_lim == 0.0f || p == 0.0f) {
+        return error*p;
+    }
+
+    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;
+    }
+}

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -162,6 +162,9 @@ public:
     // lean_angle_max - maximum lean angle of the copter in centi-degrees
     int16_t lean_angle_max() const { return _aparm.angle_max; }
 
+    // sqrt_controller - response based on the sqrt of the error instead of the more common linear response
+    static float sqrt_controller(float error, float p, float second_ord_lim);
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];