From f23e94707c96723f8d4b47680dfdf54daab4b2dd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Jun 2014 17:55:31 +0900 Subject: [PATCH] AC_WPNav: use fast_atan2 for bearing calcs --- libraries/AC_WPNav/AC_WPNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index fdbb7a726e..a1dff9f278 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -822,7 +822,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) _pos_control.set_pos_target(target_pos); // update the yaw - _yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)); + _yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x)); // advance spline time to next step _spline_time += _spline_time_scale*dt; @@ -861,7 +861,7 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector // To-Do: move this to math library float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const { - float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f; + float bearing = 9000 + fast_atan2(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f; if (bearing < 0) { bearing += 36000; }