diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 35980b4e21..8723c83f36 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -1336,10 +1336,15 @@ void update_alt()
 		sonar_alt = (float)sonar_alt * temp;
 		*/
 
-		scale = (sonar_alt - 300) / 300;
-		scale = constrain(scale, 0, 1);
+		if(baro_alt < 800){
+			scale = (sonar_alt - 300) / 300;
+			scale = constrain(scale, 0, 1);
 
-		current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+			current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+
+		}else{
+			current_loc.alt = baro_alt + home.alt;
+		}
 
 	}else{
 		baro_alt 		= read_barometer();
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index c1c06888d4..f0da9ea954 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -388,6 +388,8 @@
 	# define YAW_IMAX				1			// degrees * 100
 	#endif
 #endif
+
+
 //////////////////////////////////////////////////////////////////////////////
 // Autopilot control limits
 //
@@ -407,7 +409,7 @@
 # define NAV_LOITER_I			0.08		// upped a bit to deal with wind faster
 #endif
 #ifndef NAV_LOITER_D
-# define NAV_LOITER_D			0.03		// Added some D 2.20, untested
+# define NAV_LOITER_D			0.15		//
 #endif
 #ifndef NAV_LOITER_IMAX
 # define NAV_LOITER_IMAX		20			// 20°
@@ -422,7 +424,7 @@
 # define NAV_WP_I				0.5			// this is a fast ramp up
 #endif
 #ifndef NAV_WP_D
-# define NAV_WP_D				0			// slight dampening of a few degrees at most
+# define NAV_WP_D				.3			//
 #endif
 #ifndef NAV_WP_IMAX
 # define NAV_WP_IMAX			40			// degrees
@@ -430,7 +432,7 @@
 
 
 #ifndef WAYPOINT_SPEED
-# define WAYPOINT_SPEED				450			// for 4.5 ms error = 13.5 pitch
+# define WAYPOINT_SPEED			600			// for 6m/s error = 13mph
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -468,10 +470,10 @@
 // Crosstrack compensation
 //
 #ifndef XTRACK_GAIN
-# define XTRACK_GAIN          5 // deg/m
+# define XTRACK_GAIN          2 // deg/m
 #endif
 #ifndef XTRACK_ENTRY_ANGLE
-# define XTRACK_ENTRY_ANGLE   40 // deg
+# define XTRACK_ENTRY_ANGLE   8 // deg
 #endif
 
 
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 666b7c139c..1e709338b2 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.30 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.31 Beta", main_menu_commands);
 
 void init_ardupilot()
 {