diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 7d18736a5c9c5b960d2f7fc58568bd48e56d53c0..f7b89263e7780508e00aa8fdae761611cd39f76f 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -103,8 +103,8 @@ static void run_autopilot()
 // set_nav_mode - update nav mode and initialise any variables as required
 static bool set_nav_mode(uint8_t new_nav_mode)
 {
-    // boolean to ensure proper initialisation of nav modes
-    bool nav_initialised = false;
+    bool nav_initialised = false;       // boolean to ensure proper initialisation of nav modes
+    Vector3f stopping_point;            // stopping point for circle mode
 
     // return immediately if no change
     if( new_nav_mode == nav_mode ) {
@@ -119,13 +119,14 @@ static bool set_nav_mode(uint8_t new_nav_mode)
 
         case NAV_CIRCLE:
             // set center of circle to current position
-            circle_set_center(inertial_nav.get_position(), ahrs.yaw);
+            wp_nav.get_stopping_point(inertial_nav.get_position(),inertial_nav.get_velocity(),stopping_point);
+            circle_set_center(stopping_point,ahrs.yaw);
             nav_initialised = true;
             break;
 
         case NAV_LOITER:
             // set target to current position
-            wp_nav.set_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+            wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
             nav_initialised = true;
             break;
 
@@ -217,10 +218,11 @@ static void
 circle_set_center(const Vector3f current_position, float heading_in_radians)
 {
     float max_velocity;
+    float cir_radius = g.circle_radius * 100;
 
     // set circle center to circle_radius ahead of current position
-    circle_center.x = current_position.x + (float)g.circle_radius * 100 * cos_yaw;
-    circle_center.y = current_position.y + (float)g.circle_radius * 100 * sin_yaw;
+    circle_center.x = current_position.x + cir_radius * cos_yaw;
+    circle_center.y = current_position.y + cir_radius * sin_yaw;
 
     // if we are doing a panorama set the circle_angle to the current heading
     if( g.circle_radius <= 0 ) {
@@ -248,6 +250,9 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
     // initialise other variables
     circle_angle_total = 0;
     circle_angular_velocity = 0;
+
+    // initialise loiter target.  Note: feed forward velocity set to zero
+    wp_nav.init_loiter_target(current_position, Vector3f(0,0,0));
 }
 
 // update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index f8d74a97871be902e484451e1593a7dee5b27c4d..f80be650f005264c85ef8ca947a9d69ab65635cc 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -143,8 +143,8 @@ void AC_WPNav::set_loiter_target(const Vector3f& position)
     _target_vel.y = 0;
 }
 
-/// set_loiter_target - set initial loiter target based on current position and velocity
-void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
+/// init_loiter_target - set initial loiter target based on current position and velocity
+void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velocity)
 {
     // set target position and velocity based on current pos and velocity
     _target.x = position.x;
@@ -155,6 +155,13 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
     // initialise desired roll and pitch to current roll and pitch.  This avoids a random twitch between now and when the loiter controller is first run
     _desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
     _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+
+    // initialise pilot input
+    _pilot_vel_forward_cms = 0;
+    _pilot_vel_right_cms = 0;
+
+    // set last velocity to current velocity
+    _vel_last = _inav->get_velocity();
 }
 
 /// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 2fb6a7d06b9e02274bd2b7f820ad93339265dd31..9901281c54050beafc5642a26f37e55f839f96a6 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -50,8 +50,8 @@ public:
     /// set_loiter_target in cm from home
     void set_loiter_target(const Vector3f& position);
 
-    /// set_loiter_target - set initial loiter target based on current position and velocity
-    void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
+    /// init_loiter_target - set initial loiter target based on current position and velocity
+    void init_loiter_target(const Vector3f& position, const Vector3f& velocity);
 
     /// move_loiter_target - move destination using pilot input
     void move_loiter_target(float control_roll, float control_pitch, float dt);