From d8ff5dcfabf6a216400a7457a8c0057ca15faca3 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Thu, 12 Jul 2012 09:22:20 -0700
Subject: [PATCH] Arducopter: Fix circle_WP

---
 ArduCopter/ArduCopter.pde     | 7 +++++--
 ArduCopter/commands_logic.pde | 5 ++---
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 2125fe85f0..fadb06ea8e 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -2404,8 +2404,11 @@ static void update_auto_yaw()
 	if(wp_control == LOITER_MODE)
 		return;
 
-	// this tracks a location so the copter is always pointing towards it.
-	if(yaw_tracking == MAV_ROI_LOCATION){
+	if(control_mode == CIRCLE_MODE){
+		auto_yaw = get_bearing(&current_loc, &circle_WP);
+
+	}else if(yaw_tracking == MAV_ROI_LOCATION){
+		// this tracks a location so the copter is always pointing towards it.
 		auto_yaw = get_bearing(&current_loc, &target_WP);
 
 	}else if(yaw_tracking == MAV_ROI_WPNEXT){
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 3305402b17..5e6db1512d 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -310,9 +310,6 @@ static void do_loiter_turns()
 {
 	wp_control = CIRCLE_MODE;
 
-	// reset desired location
-
-
 	if(command_nav_queue.lat == 0){
 		// allow user to specify just the altitude
 		if(command_nav_queue.alt > 0){
@@ -323,6 +320,8 @@ static void do_loiter_turns()
 		set_next_WP(&command_nav_queue);
 	}
 
+	circle_WP = next_WP;
+
 	loiter_total = command_nav_queue.p1 * 360;
 	loiter_sum	 = 0;
 	old_target_bearing = target_bearing;