From a2886fc9915e65335e9912b14524897f6949360f Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@gmail.com>
Date: Sat, 24 Sep 2011 21:49:45 -0700
Subject: [PATCH] turned off Stabilize_I by default because it was hurting
 loiter tweaked speed control of rate_nav

---
 ArduCopter/config.h       | 10 +++++-----
 ArduCopter/navigation.pde | 12 ++++++++----
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 9f5bfb54cb..ad31613024 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -406,20 +406,20 @@
 # define STABILIZE_ROLL_P 		4.2
 #endif
 #ifndef STABILIZE_ROLL_I
-# define STABILIZE_ROLL_I 		0.008
+# define STABILIZE_ROLL_I 		0.001
 #endif
 #ifndef STABILIZE_ROLL_IMAX
-# define STABILIZE_ROLL_IMAX 	3		// degrees
+# define STABILIZE_ROLL_IMAX 	0		// degrees
 #endif
 
 #ifndef STABILIZE_PITCH_P
 # define STABILIZE_PITCH_P		4.2
 #endif
 #ifndef STABILIZE_PITCH_I
-# define STABILIZE_PITCH_I		0.008
+# define STABILIZE_PITCH_I		0.001
 #endif
 #ifndef STABILIZE_PITCH_IMAX
-# define STABILIZE_PITCH_IMAX	3		// degrees
+# define STABILIZE_PITCH_IMAX	0		// degrees
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -502,7 +502,7 @@
 #endif
 
 #ifndef WAYPOINT_SPEED_MAX
-# define WAYPOINT_SPEED_MAX			450			// for 6m/s error = 13mph
+# define WAYPOINT_SPEED_MAX			400			// for 6m/s error = 13mph
 #endif
 
 
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index c437ef18f7..bc5c35e2e5 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -116,12 +116,16 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
 	nav_lon		 	= constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
 }
 
-#define NAV2_ERR_MAX 600.0
 static void calc_nav_rate2(int max_speed)
 {
-	float scaler 	= (float)wp_distance / NAV2_ERR_MAX;
-	scaler			= constrain(scaler, 0.0, 1.0);
-	max_speed 		= (float)max_speed * scaler;
+	/*
+	0  1   2   3   4   5   6   7   8
+	...|...|...|...|...|...|...|...|
+		  100	  200	  300	  400
+	                                     +|+
+	*/
+
+	max_speed 		= min(max_speed, (wp_distance * 50));
 
 	// XXX target_angle should be the original  desired target angle!
 	float temp		= radians((original_target_bearing - g_gps->ground_course)/100.0);