AC_AttControl: add sqrt_controller

Original code by Jonathan Challinger
AC_AttitudeControl: use sqrt_controller function
AC_PosControl: Use sqrt_controller function
AC_AttitudeControl: cleanup angle_ef_roll_pitch_rate_ef_yaw_smooth
AC_PosControl: use AttControl's sqrt_controller