diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index da472a1f65..33de308932 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -217,7 +217,7 @@ public:
 	AP_Int8		super_simple;
 	AP_Int8		rtl_land_enabled;
 	AP_Int16	rtl_approach_alt;
-	AP_Float	tilt_comp;
+	AP_Int8		tilt_comp;
 	AP_Int8		axis_enabled;
 	AP_Int8		copter_leds_mode;	// Operating mode of LED lighting system
 
@@ -341,7 +341,7 @@ public:
 	super_simple			(SUPER_SIMPLE),
 	rtl_land_enabled		(0),
 	rtl_approach_alt		(RTL_APPROACH_ALT),
-	tilt_comp				(.0054),
+	tilt_comp				(54),
 	axis_enabled			(AXIS_LOCK_ENABLED),
 	copter_leds_mode		(9),
 
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index a40790e54d..ddfd44abed 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -734,13 +734,13 @@
 // WP Navigation control gains
 //
 #ifndef NAV_P
-# define NAV_P				2.4			//
+# define NAV_P				2.2			//
 #endif
 #ifndef NAV_I
 # define NAV_I				0.17		// Wind control
 #endif
 #ifndef NAV_D
-# define NAV_D				0.00		//
+# define NAV_D				0.00		// .95
 #endif
 #ifndef NAV_IMAX
 # define NAV_IMAX			18			// degrees
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 70df04638a..f6d27ed122 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -173,7 +173,7 @@ static void calc_loiter(int x_error, int y_error)
 	g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator());
 }
 
-static void calc_nav_rate(int max_speed)
+static void calc_nav_rate(int16_t max_speed)
 {
 	float temp, temp_x, temp_y;
 
@@ -191,19 +191,17 @@ static void calc_nav_rate(int max_speed)
 	temp_y 			= sin(temp);
 
 	// rotate desired spped vector:
-	int16_t x_target_speed = max_speed   * temp_x - cross_speed * temp_y;
-	int16_t y_target_speed = cross_speed * temp_x + max_speed   * temp_y;
+	int32_t x_target_speed = max_speed   * temp_x - cross_speed * temp_y;
+	int32_t y_target_speed = cross_speed * temp_x + max_speed   * temp_y;
 
 	// East / West
 	x_rate_error 	= x_target_speed - x_actual_speed; // 413
 	x_rate_error 	= constrain(x_rate_error, -1000, 1000);
 	nav_lon			= g.pid_nav_lon.get_pid(x_rate_error, dTnav);
-	temp			= x_target_speed * x_target_speed;
-	temp			*= g.tilt_comp;
+	int32_t tilt	= (x_target_speed * x_target_speed * (int32_t)g.tilt_comp) / 10000;
 
-	if(x_target_speed < 0)
-		temp = -temp;
-	nav_lon			+= temp;
+	if(x_target_speed < 0) tilt = -tilt;
+	nav_lon			+= tilt;
 	nav_lon			= constrain(nav_lon, -3000, 3000);
 
 
@@ -211,12 +209,10 @@ static void calc_nav_rate(int max_speed)
 	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.pid_nav_lat.get_pid(y_rate_error, dTnav);
-	temp			= y_target_speed * y_target_speed;
-	temp			*= g.tilt_comp;
+	tilt			= (y_target_speed * y_target_speed * (int32_t)g.tilt_comp) / 10000;
 
-	if(y_target_speed < 0)
-		temp = -temp;
-	nav_lat			+= temp;
+	if(y_target_speed < 0)	tilt = -tilt;
+	nav_lat			+= tilt;
 	nav_lat			= constrain(nav_lat, -3000, 3000);
 
 	// copy over I term to Loiter_Rate