diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index d7233c32eb..3505321df2 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -475,8 +475,12 @@ static int32_t	nav_pitch;							// deg * 100 : target pitch angle
 static int32_t	nav_yaw;							// deg * 100 : target yaw angle
 static int32_t	home_to_copter_bearing;				// deg * 100 : target yaw angle
 static int32_t	auto_yaw;							// deg * 100 : target yaw angle
-static int32_t	nav_lat;							// for error calcs
-static int32_t	nav_lon;							// for error calcs
+
+static int16_t	nav_lat;							// for error calcs
+static int16_t	nav_lon;							// for error calcs
+static int16_t	nav_lat_p;							// for error calcs
+static int16_t	nav_lon_p;							// for error calcs
+
 static int16_t	nav_throttle;						// 0-1000 for throttle control
 static int16_t	crosstrack_error;
 
@@ -759,6 +763,7 @@ static void medium_loop()
 						y_GPS_speed 	= optflow.y_cm;
 					}
 					#endif
+
 					// control mode specific updates
 					// -----------------------------
 					update_navigation();
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 63dd139356..89e9790ef0 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -606,10 +606,10 @@
 // Navigation control gains
 //
 #ifndef LOITER_P
-# define LOITER_P			.2		//
+# define LOITER_P			.2		// .3 was too aggressive
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.03	// Wind control
+# define LOITER_I			0.05	// Wind control
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees°
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 82acca9463..316458f7ba 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -47,6 +47,7 @@ static void calc_XY_velocity(){
 	// this speed is ~ in cm because we are using 10^7 numbers from GPS
 	float tmp = 1.0/dTnav;
 	//int8_t tmp = 5;
+
 	int16_t	x_diff	= (g_gps->longitude - last_longitude) * tmp;
 	int16_t	y_diff	= (g_gps->latitude  - last_latutude) * tmp;
 
@@ -54,13 +55,12 @@ static void calc_XY_velocity(){
 	x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
 	y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
 
-	if(g_gps->ground_speed > 150){
+	if(g_gps->ground_speed > 120){
 		// Derive X/Y speed from GPS
 		// this is far more accurate when traveling about 1.5m/s
 		float temp		= g_gps->ground_course * RADX100;
 		x_GPS_speed 	= sin(temp) * (float)g_gps->ground_speed;
 		y_GPS_speed 	= cos(temp) * (float)g_gps->ground_speed;
-
 	}
 
 	last_longitude 	= g_gps->longitude;
@@ -90,41 +90,42 @@ static void calc_location_error(struct Location *next_loc)
 #define NAV_ERR_MAX 800
 static void calc_loiter(int x_error, int y_error)
 {
-	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
-	y_error 				= constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
-
-	int16_t y_target_speed 	= g.pi_loiter_lat.get_p(y_error);
-	int16_t y_iterm 		= g.pi_loiter_lat.get_i(y_error, dTnav);
-	y_rate_error 			= y_target_speed - y_actual_speed; // 413
-	y_rate_error 			= constrain(y_rate_error, -1000, 1000);	// added a rate error limit to keep pitching down to a minimum
-	nav_lat		 			= g.pi_nav_lat.get_p(y_rate_error);
-	nav_lat					= constrain(nav_lat, -3500, 3500);
-	nav_lat					+= y_iterm;
-
+	// East/West
+	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);	//800
 	int16_t x_target_speed 	= g.pi_loiter_lon.get_p(x_error);
 	int16_t x_iterm 		= g.pi_loiter_lon.get_i(x_error, dTnav);
 	x_rate_error 			= x_target_speed - x_actual_speed;
 	x_rate_error 			= constrain(x_rate_error, -1000, 1000);
-	nav_lon		 			= g.pi_nav_lon.get_p(x_rate_error);
-	nav_lon					= constrain(nav_lon, -3500, 3500);
-	nav_lon					+= x_iterm;
+	nav_lon_p		 		= g.pi_nav_lon.get_p(x_rate_error);
+	nav_lon_p				= constrain(nav_lon_p, -3500, 3500);
+	nav_lon					= nav_lon_p + x_iterm;
 
-	/*
+	// North/South
+	y_error 				= constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
+	int16_t y_target_speed 	= g.pi_loiter_lat.get_p(y_error);
+	int16_t y_iterm 		= g.pi_loiter_lat.get_i(y_error, dTnav);
+	y_rate_error 			= y_target_speed - y_actual_speed; // 413
+	y_rate_error 			= constrain(y_rate_error, -1000, 1000);	// added a rate error limit to keep pitching down to a minimum
+	nav_lat_p	 			= g.pi_nav_lat.get_p(y_rate_error);
+	nav_lat_p				= constrain(nav_lat_p, -3500, 3500);
+	nav_lat					= nav_lat_p + y_iterm;
+
+	///*
 	int8_t ttt = 1.0/dTnav;
 	int16_t t2 = g.pi_nav_lat.get_integrator();
-				// 1   2   3   4   5    6   7  8   9   10	11
-	Serial.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+
+				// 1   2   3      4   5   6   7   8   9   10
+	Serial.printf("%d, %d, %d,    %d, %d, %d, %d, %d, %d, %d\n",
 					wp_distance, 	//1
-					ttt,			//2
-					y_error,		//3
-					y_GPS_speed,	//4
-					y_GPS_speed2,	//5
-					y_actual_speed,	//6
-					y_target_speed,	//7
-					y_rate_error,	//8
-					nav_lat,		//9
-					y_iterm,		//10
-					t2);			//11
+					y_error,		//2
+					y_GPS_speed,	//3
+					y_actual_speed,	//4  ;
+					y_target_speed,	//5
+					y_rate_error,	//6
+					nav_lat_p,		//7
+					nav_lat,		//8
+					y_iterm,		//9
+					t2);			//10
 	//*/
 
 	/*
@@ -143,7 +144,7 @@ static void calc_loiter(int x_error, int y_error)
 	//*/
 }
 
-//wp_distance, y_error, y_GPS_speed, y_GPS_speed2, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
+//wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
 
 
 #define ERR_GAIN .01