diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 5a0a63b923..8059e7f3c0 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -327,7 +327,6 @@ static long		target_bearing;						// deg * 100 : 0 to 360 location of the plane
 //static long		crosstrack_correction;				// deg * 100 : 0 to 360 desired angle of plane to target
 
 static int		climb_rate;							// m/s * 100  - For future implementation of controlled ascent/descent by rate
-static long 	circle_angle = 0;
 static byte	wp_control;							// used to control - navgation or loiter
 
 static byte	command_must_index;					// current command memory location
@@ -402,9 +401,15 @@ static byte 	yaw_tracking = MAV_ROI_WPNEXT;		// no tracking, point at next wp, o
 
 // Loiter management
 // -----------------
-static long 	saved_target_bearing;				// deg * 100
-static unsigned long 	loiter_time;				// millis : when we started LOITER mode
-static unsigned long 	loiter_time_max;			// millis : how long to stay in LOITER mode
+static long 	original_target_bearing;			// deg * 100, used to check we are not passing the WP
+static long 	old_target_bearing;					// used to track difference in angle
+
+static int		loiter_total; 						// deg : how many times to loiter * 360
+//static int 		loiter_delta;						// deg : how far we just turned
+static int		loiter_sum;							// deg : how far we have turned around a waypoint
+static long 	loiter_time;						// millis : when we started LOITER mode
+static int 		loiter_time_max;					// millis : how long to stay in LOITER mode
+
 
 // these are the values for navigation control functions
 // ----------------------------------------------------
@@ -1135,7 +1140,6 @@ static void update_navigation()
 	switch(control_mode){
 		case AUTO:
 			verify_commands();
-
 			// note: wp_control is handled by commands_logic
 
 			// calculates desired Yaw
@@ -1147,6 +1151,7 @@ static void update_navigation()
 
 		case GUIDED:
 			wp_control = WP_MODE;
+
 			update_auto_yaw();
 			update_nav_wp();
 			break;
@@ -1161,6 +1166,7 @@ static void update_navigation()
 
 				wp_control = WP_MODE;
 			}else{
+				// lets just jump to Loiter Mode after RTL
 				set_mode(LOITER);
 				//xtrack_enabled = false;
 			}
@@ -1171,33 +1177,19 @@ static void update_navigation()
 
 			// switch passthrough to LOITER
 		case LOITER:
-			wp_control = LOITER_MODE;
+			wp_control 		= LOITER_MODE;
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
 			break;
 
 		case CIRCLE:
-			yaw_tracking = MAV_ROI_WPNEXT;
+			yaw_tracking	= MAV_ROI_WPNEXT;
+			wp_control 		= CIRCLE_MODE;
 
 			// calculates desired Yaw
 			update_auto_yaw();
-			{
-				circle_angle = wrap_360(target_bearing + 3000 + 18000);
-
-				target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));
-				target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100)));
-			}
-
-			// calc the lat and long error to the target
-			calc_location_error(&target_WP);
-
-			// use error as the desired rate towards the target
-			// nav_lon, nav_lat is calculated
-			calc_nav_rate(long_error, lat_error, 200, 0);
-
-			// rotate pitch and roll to the copter frame of reference
-			calc_nav_pitch_roll();
+			update_nav_wp();
 			break;
 
 	}
@@ -1383,6 +1375,39 @@ static void update_nav_wp()
 		// rotate pitch and roll to the copter frame of reference
 		calc_nav_pitch_roll();
 
+	}else if(wp_control == CIRCLE_MODE){
+
+		// check if we have missed the WP
+		int loiter_delta = (target_bearing - old_target_bearing)/100;
+
+		// reset the old value
+		old_target_bearing = target_bearing;
+
+		// wrap values
+		if (loiter_delta > 180) loiter_delta -= 360;
+		if (loiter_delta < -180) loiter_delta += 360;
+
+		// sum the angle around the WP
+		loiter_sum += abs(loiter_delta);
+
+
+		// creat a virtual waypoint that circles the next_WP
+		// Count the degrees we have circulated the WP
+		int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100;
+
+		target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle)));
+		target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle)));
+
+		// calc the lat and long error to the target
+		calc_location_error(&target_WP);
+
+		// use error as the desired rate towards the target
+		// nav_lon, nav_lat is calculated
+		calc_nav_rate(long_error, lat_error, 200, 0);
+
+		// rotate pitch and roll to the copter frame of reference
+		calc_nav_pitch_roll();
+
 	} else {
 		// for long journey's reset the wind resopnse
 		// it assumes we are standing still.
diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index cc615e5382..31faf10d3c 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -176,11 +176,7 @@ static void set_next_WP(struct Location *wp)
 
 	// to check if we have missed the WP
 	// ----------------------------
-	saved_target_bearing = target_bearing;
-
-	// set a new crosstrack bearing
-	// ----------------------------
-	//crosstrack_bearing 	= target_bearing;	// Used for track following
+	original_target_bearing = target_bearing;
 
 	gcs.print_current_waypoints();
 }
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index b20649c1d6..6adaed22f2 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -142,7 +142,7 @@ static bool verify_must()
 			break;
 
 		case MAV_CMD_NAV_LOITER_TURNS:
-			return true;
+			return verify_loiter_turns();
 			break;
 
 		case MAV_CMD_NAV_LOITER_TIME:
@@ -297,8 +297,7 @@ static void do_loiter_unlimited()
 
 static void do_loiter_turns()
 {
-/*
-	wp_control = LOITER_MODE;
+	wp_control == CIRCLE_MODE;
 
 	if(next_command.lat == 0)
 		set_next_WP(&current_loc);
@@ -306,7 +305,7 @@ static void do_loiter_turns()
 		set_next_WP(&next_command);
 
 	loiter_total = next_command.p1 * 360;
-*/
+	loiter_sum	 = 0;
 }
 
 static void do_loiter_time()
@@ -431,6 +430,20 @@ static bool verify_loiter_time()
 	return false;
 }
 
+static bool verify_loiter_turns()
+{
+	// have we rotated around the center enough times?
+	// -----------------------------------------------
+	if(loiter_sum > loiter_total) {
+		loiter_total 	= 0;
+		loiter_sum		= 0;
+		//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
+		// clear the command queue;
+		return true;
+	}
+	return false;
+}
+
 static bool verify_RTL()
 {
 	if (wp_distance <= g.waypoint_radius) {
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 4da914e137..58070376dc 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -165,6 +165,7 @@
 
 #define LOITER_MODE 1
 #define WP_MODE 2
+#define CIRCLE_MODE 3
 
 // Waypoint options
 #define WP_OPTION_ALT_RELATIVE 		1
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 64076b7aa8..38386c289f 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -28,7 +28,7 @@ static byte navigate()
 
 static bool check_missed_wp()
 {
-	long temp 	= target_bearing - saved_target_bearing;
+	long temp 	= target_bearing - original_target_bearing;
 	temp 		= wrap_180(temp);
 	return (abs(temp) > 10000);	//we pased the waypoint by 10 °
 }