AC_AttitudeControl: use AP_Math control functions

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -847,69 +847,6 @@ float AC_AttitudeControl::get_althold_lean_angle_max() const
     return MAX(ToDeg(_althold_lean_angle_max), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN) * 100.0f;
 }
 
-// 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 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;
-        }
-    } 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;
-    }
-}
-
-// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
-float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim)
-{
-    if (is_positive(second_ord_lim) && !is_zero(second_ord_lim) && is_zero(p)) {
-        return (first_ord_mag * first_ord_mag) / (2.0f * second_ord_lim);
-    } else if ((is_negative(second_ord_lim) || is_zero(second_ord_lim)) && !is_zero(p)) {
-        return first_ord_mag / p;
-    } else if ((is_negative(second_ord_lim) || is_zero(second_ord_lim)) && is_zero(p)) {
-        return 0.0f;
-    }
-
-    // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
-    float linear_velocity = second_ord_lim / p;
-
-    if (fabsf(first_ord_mag) < linear_velocity) {
-        // if our current velocity is below the cross-over point we use a linear function
-        return first_ord_mag / p;
-    } else {
-        float linear_dist = second_ord_lim / sq(p);
-        float overshoot = (linear_dist * 0.5f) + sq(first_ord_mag) / (2.0f * second_ord_lim);
-        if (is_positive(first_ord_mag)) {
-            return overshoot;
-        } else {
-            return -overshoot;
-        }
-    }
-}
-
 // Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
 float AC_AttitudeControl::max_rate_step_bf_roll()
 {

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -268,12 +268,6 @@ public:
     // Return tilt angle in degrees
     float lean_angle() const { return degrees(_thrust_angle); }
 
-    // Proportional controller with piecewise sqrt sections to constrain second derivative
-    static float sqrt_controller(float error, float p, float second_ord_lim, float dt);
-
-    // Inverse proportional controller with piecewise sqrt sections to constrain second derivative
-    static float stopping_point(float first_ord_mag, float p, float second_ord_lim);
-
     // calculates the velocity correction from an angle error. The angular velocity has acceleration and
     // deceleration limits including basic jerk limiting using smoothing_gain
     static float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt);