AP_Math: add sqrt_controller function
AC_AttitudeControl: use sqrt controller function