diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 0b09fe62d9..48accacea9 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -246,7 +246,7 @@ void AC_Circle::init_start_angle(bool use_heading) _angle = wrap_PI(_ahrs.yaw-PI); } else { // get bearing from circle center to vehicle in radians - float bearing_rad = ToRad(90) + fast_atan2(-(curr_pos.x-_center.x), curr_pos.y-_center.y); + float bearing_rad = fast_atan2(curr_pos.y-_center.y,curr_pos.x-_center.x); _angle = wrap_PI(bearing_rad); } }