diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp
index 3f92b7ffca..a485d46b39 100644
--- a/libraries/AC_WPNav/AC_Circle.cpp
+++ b/libraries/AC_WPNav/AC_Circle.cpp
@@ -40,10 +40,10 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-/// set_circle_center in cm from home
-void AC_Circle::set_center(const Vector3f& position)
+/// init - initialise circle controller setting center specifically
+void AC_Circle::init(const Vector3f& center)
 {
-    _center = position;
+    _center = center;
 
 	// To-Do: set target position, angle, etc so that copter begins circle from closest point to stopping point
 	_pos_control.set_pos_target(_inav.get_position());
@@ -52,10 +52,13 @@ void AC_Circle::set_center(const Vector3f& position)
 
     // calculate velocities
     calc_velocities();
+
+    // set start angle from position
+    init_start_angle(false);
 }
 
-/// init_center in cm from home using stopping point and projecting out based on the copter's heading
-void AC_Circle::init_center()
+/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
+void AC_Circle::init()
 {
     Vector3f stopping_point;
 
@@ -73,6 +76,9 @@ void AC_Circle::init_center()
 
     // calculate velocities
     calc_velocities();
+
+    // set starting angle from vehicle heading
+    init_start_angle(true);
 }
 
 /// update - update circle controller
@@ -146,6 +152,42 @@ void AC_Circle::update()
     _pos_control.update_pos_controller(false);
 }
 
+// get_closest_point_on_circle - returns closest point on the circle
+//  circle's center should already have been set
+//  closest point on the circle will be placed in result
+//  result's altitude (i.e. z) will be set to the circle_center's altitude
+//  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
+void AC_Circle::get_closest_point_on_circle(Vector3f &result)
+{
+    // return center if radius is zero
+    if (_radius <= 0) {
+        result = _center;
+        return;
+    }
+
+    // get current position
+    const Vector3f &curr_pos = _inav.get_position();
+
+    // calc vector from current location to circle center
+    Vector2f vec;   // vector from circle center to current location
+    vec.x = (curr_pos.x - _center.x);
+    vec.y = (curr_pos.y - _center.y);
+    float dist = pythagorous2(vec.x, vec.y);
+
+    // if current location is exactly at the center of the circle return edge directly behind vehicle
+    if (dist == 0) {
+        result.x = _center.x - _radius * _ahrs.cos_yaw();
+        result.y = _center.y - _radius * _ahrs.sin_yaw();
+        result.z = _center.z;
+        return;
+    }
+
+    // calculate closest point on edge of circle
+    result.x = _center.x + vec.x / dist * _radius;
+    result.y = _center.y + vec.y / dist * _radius;
+    result.z = _center.z;
+}
+
 // calc_velocities - calculate angular velocity max and acceleration based on radius and rate
 //      this should be called whenever the radius or rate are changed
 //      initialises the yaw and current position around the circle
@@ -153,7 +195,6 @@ void AC_Circle::calc_velocities()
 {
     // if we are doing a panorama set the circle_angle to the current heading
     if (_radius <= 0) {
-        _angle = _ahrs.yaw;
         _angular_vel_max = ToRad(_rate);
         _angular_accel = _angular_vel_max;  // reach maximum yaw velocity in 1 second
     }else{
@@ -174,7 +215,36 @@ void AC_Circle::calc_velocities()
         }
     }
 
-    // initialise other variables
-    _angle_total = 0;
+    // initialise angular velocity
     _angular_vel = 0;
 }
+
+// init_start_angle - sets the starting angle around the circle and initialises the angle_total
+//  if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
+//  if use_heading is false the vehicle's position from the center will be used to initialise the angle
+void AC_Circle::init_start_angle(bool use_heading)
+{
+    // initialise angle total
+    _angle_total = 0;
+
+    // if the radius is zero we are doing panorama so init angle to the current heading
+    if (_radius <= 0) {
+        _angle = _ahrs.yaw;
+        return;
+    }
+
+    // if use_heading is true
+    if (use_heading) {
+        _angle = wrap_PI(_ahrs.yaw-PI);
+    } else {
+        // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
+        const Vector3f &curr_pos = _inav.get_position();
+        if (curr_pos.x == _center.x && curr_pos.y == _center.y) {
+            _angle = wrap_PI(_ahrs.yaw-PI);
+        } else {
+            // get bearing from circle center to vehicle in radians
+            float bearing_rad = ToRad(90) + atan2f(-(curr_pos.x-_center.x), curr_pos.y-_center.y);
+            _angle = wrap_PI(bearing_rad);
+        }
+    }
+}
diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h
index af11227e73..8114c46ec1 100644
--- a/libraries/AC_WPNav/AC_Circle.h
+++ b/libraries/AC_WPNav/AC_Circle.h
@@ -21,17 +21,22 @@ public:
     /// Constructor
     AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control);
 
-    /// set_circle_center in cm from home
-    void set_center(const Vector3f& position);
+    /// init - initialise circle controller setting center specifically
+    void init(const Vector3f& center);
 
-    /// init_center in cm from home using stopping point and projecting out based on the copter's heading
-    void init_center();
+    /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
+    void init();
+
+    /// set_circle_center in cm from home
+    void set_center(const Vector3f& center) { _center = center; }
 
     /// get_circle_center in cm from home
     const Vector3f& get_center() const { return _center; }
 
+    /// get_radius - returns radius of circle in cm
+    float get_radius() { return _radius; }
     /// set_radius - sets circle radius in cm
-    void set_radius(float radius_cm) { _radius = radius_cm; };
+    void set_radius(float radius_cm) { _radius = radius_cm; }
 
     /// set_circle_rate - set circle rate in degrees per second
     void set_rate(float deg_per_sec) { _rate = deg_per_sec; }
@@ -43,9 +48,16 @@ public:
     void update();
 
     /// get desired roll, pitch which should be fed into stabilize controllers
-    int32_t get_roll() const { return _pos_control.get_roll(); };
-    int32_t get_pitch() const { return _pos_control.get_pitch(); };
-    int32_t get_yaw() const { return _yaw; };
+    int32_t get_roll() const { return _pos_control.get_roll(); }
+    int32_t get_pitch() const { return _pos_control.get_pitch(); }
+    int32_t get_yaw() const { return _yaw; }
+
+    // get_closest_point_on_circle - returns closest point on the circle
+    //  circle's center should already have been set
+    //  closest point on the circle will be placed in result
+    //  result's altitude (i.e. z) will be set to the circle_center's altitude
+    //  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
+    void get_closest_point_on_circle(Vector3f &result);
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -56,6 +68,11 @@ private:
     //      initialises the yaw and current position around the circle
     void calc_velocities();
 
+    // init_start_angle - sets the starting angle around the circle and initialises the angle_total
+    //  if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
+    //  if use_heading is false the vehicle's position from the center will be used to initialise the angle
+    void init_start_angle(bool use_heading);
+
     // flags structure
     struct circle_flags {
         uint8_t panorama    : 1;    // true if we are doing a panorama