diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 2f49c5c4c2..fd8d49d41c 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.1.1r9 alpha"
+#define THISFIRMWARE "ArduCopter V2.1.1r10 alpha"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 4896d0a5b4..90bfeed52d 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -359,9 +359,12 @@ static bool verify_land()
 	// remenber altitude for climb_rate
 	old_alt = current_loc.alt;
 
-	if ((current_loc.alt - home.alt) < 200){
+	if ((current_loc.alt - home.alt) < 250){
 		// don't bank to hold position
 		wp_control = NO_NAV_MODE;
+		// try and come down faster
+		// by setting next_WP really low.
+		set_new_altitude(-1000);
 	}
 
 	if((current_loc.alt - home.alt)  < 100 && velocity_land <= 50){
diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde
index f2cc90d52f..eb1c46aaa3 100644
--- a/ArduCopter/commands_process.pde
+++ b/ArduCopter/commands_process.pde
@@ -66,7 +66,6 @@ static void update_commands()
 				// We are still in the same mode as what landed us,
 				// so maybe we try to continue to descend just in case we are still in the air
 				// This will also drive down the Iterm to -300
-				set_new_altitude(-10000);
 
 				// We can't disarm the motors easily. We could very well be wrong
 				//
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 60cbb35f7b..cde96d43f0 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -615,10 +615,10 @@
 
 
 //////////////////////////////////////////////////////////////////////////////
-// Navigation control gains
+// Loiter control gains
 //
 #ifndef LOITER_P
-# define LOITER_P			.25		//
+# define LOITER_P			2.4		// was .25 in previous
 #endif
 #ifndef LOITER_I
 # define LOITER_I			0.1	// Wind control
@@ -627,6 +627,9 @@
 # define LOITER_IMAX		30		// degrees°
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// WP Navigation control gains
+//
 #ifndef NAV_P
 # define NAV_P				2.2			// 3 was causing rapid oscillations in Loiter
 #endif
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 66521347b6..15ca34278c 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -44,7 +44,7 @@ static void calc_XY_velocity(){
 	// used for estimations below 1.5m/s
 	// our GPS is about 1m per
 	static int32_t last_longitude = 0;
-	static int32_t last_latutude  = 0;
+	static int32_t last_latitude  = 0;
 
 	// y_GPS_speed positve = Up
 	// x_GPS_speed positve = Right
@@ -54,11 +54,14 @@ static void calc_XY_velocity(){
 	//int8_t tmp = 5;
 
 	int16_t	x_diff	= (g_gps->longitude - last_longitude) * tmp;
-	int16_t	y_diff	= (g_gps->latitude  - last_latutude) * tmp;
+	int16_t	y_diff	= (g_gps->latitude  - last_latitude) * tmp;
 
 	// filter
-	x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
-	y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
+	x_GPS_speed = (x_GPS_speed  + x_diff) >> 1;
+	y_GPS_speed = (y_GPS_speed  + y_diff) >> 1;
+
+	//x_GPS_speed = x_diff;
+	//y_GPS_speed = y_diff;
 
 	// Above simply works better than GPS groundspeed
 	// which is proving to be problematic
@@ -72,7 +75,7 @@ static void calc_XY_velocity(){
 	}*/
 
 	last_longitude 	= g_gps->longitude;
-	last_latutude 	= g_gps->latitude;
+	last_latitude 	= g_gps->latitude;
 
 	//Serial.printf("GS: %d  \tx:%d \ty:%d\n", g_gps->ground_speed, x_GPS_speed, y_GPS_speed);
 }
@@ -95,9 +98,68 @@ static void calc_location_error(struct Location *next_loc)
 	lat_error	= next_loc->lat - current_loc.lat;							// 500 - 0 = 500 Go North
 }
 
+/*
+//static void calc_loiter3(int x_error, int y_error)
+{
+	static int32_t	gps_lat_I = 0;
+	static int32_t	gps_lon_I = 0;
+
+	// If close to goal <1m reset the I term
+	if (abs(x_error) < 50)
+		gps_lon_I = 0;
+	if (abs(y_error) < 50)
+		gps_lat_I = 0;
+
+	gps_lon_I += x_error;
+	gps_lat_I += y_error;
+
+	gps_lon_I = constrain(gps_lon_I,-3000,3000);
+	gps_lat_I = constrain(gps_lat_I,-3000,3000);
+
+	int16_t lon_P = 1.2 	* (float)x_error;
+	int16_t lon_I = 0.1 	* (float)gps_lon_I;  //.1
+	int16_t lon_D = 3 		* x_GPS_speed ; // this controls the small bumps
+
+	int16_t lat_P = 1.2 	* (float)y_error;
+	int16_t lat_I = 0.1 	* (float)gps_lat_I;
+	int16_t lat_D = 3 		* y_GPS_speed ;
+
+	//limit of terms
+	lon_I = constrain(lon_I,-3000,3000);
+	lat_I = constrain(lat_I,-3000,3000);
+	lon_D = constrain(lon_D,-500,500);  //this controls the long distance dampimg
+	lat_D = constrain(lat_D,-500,500);  //this controls the long distance dampimg
+
+	nav_lon 	= lon_P  + lon_I - lon_D;
+	nav_lat 	= lat_P  + lat_I - lat_D;
+
+	Serial.printf("%d, %d, %d, %d, %d, %d\n",
+				lon_P, lat_P,
+				lon_I, lat_I,
+				lon_D, lat_D);
+
+}
+*/
 
 #define NAV_ERR_MAX 800
 static void calc_loiter(int x_error, int y_error)
+{
+	int16_t lon_PI 	= g.pi_loiter_lon.get_pi(x_error, dTnav);
+	int16_t lon_D 	= 3 * x_actual_speed ; // this controls the small bumps
+
+	int16_t lat_PI 	= g.pi_loiter_lat.get_pi(y_error, dTnav);
+	int16_t lat_D 	= 3 * y_actual_speed ;
+
+	//limit of terms
+	lon_D = constrain(lon_D,-500,500);
+	lat_D = constrain(lat_D,-500,500);
+
+	nav_lon		= constrain(lon_PI - lon_D, -2500, 2500);
+	nav_lat		= constrain(lat_PI - lat_D, -2500, 2500);
+}
+
+
+static void calc_loiter1(int x_error, int y_error)
 {
 	// East/West
 	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);	//800