From 468d9c4b4fdda09ea58c14d5295158d48856b067 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Tue, 13 Mar 2012 10:21:56 -0700
Subject: [PATCH] ACM: Loiter D performance updates

---
 ArduCopter/navigation.pde | 34 +++++++++++++++++++++++++---------
 1 file changed, 25 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index f5f5616275..565e642a7a 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -105,21 +105,37 @@ static void calc_location_error(struct Location *next_loc)
 
 	int16_t tmp;
 
-	tmp			= (long_error - last_lon_error);
-	if(abs(abs(tmp) -last_lon_d) > 15)	tmp = x_rate_d;
+	// -------------------------------------
+	tmp	= (long_error - last_lon_error);
+	if(abs(abs(tmp) -last_lon_d) > 20){
+		tmp = x_rate_d;
+	}
+	if(long_error > 0){
+		if(tmp < 0) tmp = 0;
+	}else{
+		if(tmp > 0) tmp = 0;
+	}
 	x_rate_d	= lon_rate_d_filter.apply(tmp);
+	x_rate_d	= constrain(x_rate_d, -800, 800);
 	last_lon_d 	= abs(tmp);
 
-	tmp			= (lat_error - last_lat_error);
-	if(abs(abs(tmp) -last_lat_d) > 15)	tmp = y_rate_d;
-	//if(abs(tmp) > 80)	tmp	= y_rate_d;
+	// -------------------------------------
+	tmp	= (lat_error - last_lat_error);
+	if(abs(abs(tmp) -last_lat_d) > 20)
+		tmp = y_rate_d;
+	if(lat_error > 0){
+		if(tmp < 0) tmp = 0;
+	}else{
+		if(tmp > 0) tmp = 0;
+	}
 	y_rate_d	= lat_rate_d_filter.apply(tmp);
+	y_rate_d	= constrain(y_rate_d, -800, 800);
 	last_lat_d 	= abs(tmp);
 
-	int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
-	int16_t raww = (long_error - last_lon_error);
-
-	//Serial.printf("XX, %d, %d, %d\n", raww, x_rate_d, t22);
+	// debug
+	//int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav);
+	//if(control_mode	== LOITER)
+	//	Serial.printf("XX, %d, %d, %d \n", long_error, t22, (int16_t)g.pid_loiter_rate_lon.get_integrator());
 
 	last_lon_error 	= long_error;
 	last_lat_error 	= lat_error;