diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 553195353c..132c03f60d 100644
--- a/ArduCopter/config.h
+++ b/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
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index bd0ef4730c..5243afbbe8 100644
--- a/ArduCopter/navigation.pde
+++ b/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