Loiter update - removed compensation curve. Did some more math and its's not worth the effort, harder to tune.

ArduCopter/config.h

@@ -637,7 +637,7 @@
 // WP Navigation control gains
 //
 #ifndef NAV_P
-# define NAV_P				2.3			// 3 was causing rapid oscillations in Loiter
+# define NAV_P				2.8			//
 #endif
 #ifndef NAV_I
 # define NAV_I				0.4			// Wind control

ArduCopter/navigation.pde

@@ -212,19 +212,19 @@ static void calc_nav_rate(int max_speed)
 static void calc_nav_lon(int rate)
 {
 	nav_lon		= g.pid_nav_lon.get_pid(rate, dTnav);
-	nav_lon		= get_corrected_angle(rate, nav_lon);
+	//nav_lon		= get_corrected_angle(rate, nav_lon);
 	nav_lon		= constrain(nav_lon, -3000, 3000);
 }
 
 static void calc_nav_lat(int rate)
 {
 	nav_lat		= g.pid_nav_lat.get_pid(rate, dTnav);
-	nav_lat		= get_corrected_angle(rate, nav_lat);
+	//nav_lat		= get_corrected_angle(rate, nav_lat);
 	nav_lat		= constrain(nav_lat, -3000, 3000);
 }
 
-static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
-{
+//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
+/*{
 	int16_t tt = desired_rate;
 	// scale down the desired rate and square it
 	desired_rate = desired_rate / 20;
@@ -240,7 +240,7 @@ static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
 	}
 	//Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp);
 	return tmp;
-}
+}*/
 
 //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2