From ec688358981d54aecf704c46dd1c9320f6a32869 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@mac.com>
Date: Fri, 9 Sep 2011 11:59:42 +1000
Subject: [PATCH] circle mode patch

---
 ArduCopter/ArduCopter.pde | 44 +++++++++++++--------------------------
 ArduCopter/navigation.pde |  6 +++---
 2 files changed, 18 insertions(+), 32 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index f1d14e547a..31d427146a 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -321,7 +321,7 @@ static long		target_bearing;						// deg * 100 : 0 to 360 location of the plane
 static bool		xtrack_enabled = false;
 static long		crosstrack_bearing;					// 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
@@ -1267,37 +1267,23 @@ static void update_navigation()
 
 			// calculates desired Yaw
 			update_auto_yaw();
+			{
+				circle_angle += dTnav; //1000 * (dTnav/1000);
+
+				if (circle_angle >= 36000)
+					circle_angle -= 36000;
+
+				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();
+			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); // move at 3m/s, minimum 2m/s
-
-			// rotate nav_lat and nav_long by nav_bearing so we circle the target
-			{
-				// rotate the desired direction based on the distance to
-				// create a vector field
-				int angle = get_loiter_angle();
-
-				// pre-calc rotation (stored in real degrees)
-				// roll
-				float cos_x = cos(radians(90 - angle));
-				// pitch
-				float sin_y = sin(radians(90 - angle));
-
-				// Rotate input by the initial bearing
-				// roll
-				long temp_roll		= nav_lon * sin_y - nav_lat * cos_x;
-				// pitch
-				long temp_pitch 	= nav_lon * cos_x + nav_lat * sin_y;
-
-				// we used temp values to not change the equations
-				// in mid-rotation
-				nav_lon = temp_roll;
-				nav_lat = temp_pitch;
-			}
+			calc_nav_rate(long_error, lat_error, 200, 0);
 
 			// rotate pitch and roll to the copter frame of reference
 			calc_nav_pitch_roll();
@@ -1475,7 +1461,7 @@ static void update_nav_wp()
 	if(wp_control == LOITER_MODE){
 
 		// calc a pitch to the target
-		calc_location_error();
+		calc_location_error(&next_WP);
 
 		// use error as the desired rate towards the target
 		calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0);
@@ -1490,7 +1476,7 @@ static void update_nav_wp()
 		g.pi_loiter_lat.reset_I();
 
 		// calc the lat and long error to the target
-		calc_location_error();
+		calc_location_error(&next_WP);
 
 		// use error as the desired rate towards the target
 		calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 100);
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 116f311d6b..6cddf96010 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -51,7 +51,7 @@ static bool check_missed_wp()
 // ------------------------------
 
 // long_error, lat_error
-static void calc_location_error()
+static void calc_location_error(struct Location *next_loc)
 {
 	/*
 	Becuase we are using lat and lon to do our distance errors here's a quick chart:
@@ -64,10 +64,10 @@ static void calc_location_error()
 	*/
 
 	// X ROLL
-	long_error	= (float)(next_WP.lng - current_loc.lng) * scaleLongDown;   // 500 - 0 = 500 roll EAST
+	long_error	= (float)(next_loc->lng - current_loc.lng) * scaleLongDown;   // 500 - 0 = 500 roll EAST
 
 	// Y PITCH
-	lat_error	= next_WP.lat - current_loc.lat;							// 0 - 500 = -500 pitch NORTH
+	lat_error	= next_loc->lat - current_loc.lat;							// 0 - 500 = -500 pitch NORTH
 }
 
 #define NAV_ERR_MAX 400