AC_Circle: fix initial angle

3drobotics · Apr 19, 2016 · fbc082a · fbc082a
1 parent 73e0852
commit fbc082a
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 7 deletions.
diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp
@@ -65,9 +65,6 @@ void AC_Circle::init(const Vector3f& center)
 
     // initialise angular velocity
     _angular_vel = 0;
-
-    // set starting angle to current heading - 180 degrees
-    _angle = wrap_PI(_ahrs.yaw-PI);
 }
 
 /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
@@ -97,9 +94,6 @@ void AC_Circle::init()
 
     // initialise angular velocity
     _angular_vel = 0;
-
-    // set starting angle to current heading - 180 degrees
-    _angle = wrap_PI(_ahrs.yaw-PI);
 }
 
 /// update - update circle controller
@@ -245,7 +239,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 = fast_atan2(curr_pos.y-_center.y,curr_pos.x-_center.x);
+            float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
             _angle = wrap_PI(bearing_rad);
         }
     }

diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h
@@ -31,6 +31,7 @@ class AC_Circle
 
     /// set_circle_center in cm from home
     void set_center(const Vector3f& center) { _center = center; }
+    void get_center(Vector3f& center) { center = _center; }
 
     /// get_circle_center in cm from home
     const Vector3f& get_center() const { return _center; }