turned off Stabilize_I by default because it was hurting loiter

tweaked speed control of rate_nav

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
 
 

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);
+	0  1   2   3   4   5   6   7   8
-	max_speed 		= (float)max_speed * scaler;
+	...|...|...|...|...|...|...|...|
+		  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);