AC_PosControl: replace safe_sqrt with pythagorous2
AC_WPNav: replace safe_sqrt with pythagorous2
InertialNav: replace safe_sqrt with pythagorous2