diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 009366cd5c..367a7e1219 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -436,10 +436,10 @@ byte 			slow_loopCounter;
 int 			superslow_loopCounter;
 byte			flight_timer;				// for limiting the execution of flight mode thingys
 
-//unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
+unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
 unsigned long 	nav2_loopTimer;				// used to track the elapsed ime for GPS nav
 
-//unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
+unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
 unsigned long 	dTnav2;						// Delta Time in milliseconds for navigation computations
 unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
@@ -589,8 +589,8 @@ void medium_loop()
 				g_gps->new_data 	= false;
 
 				// we are not tracking I term on navigation, so this isn't needed
-				//dTnav 				= millis() - nav_loopTimer;
-				//nav_loopTimer 		= millis();
+				dTnav 				= millis() - nav_loopTimer;
+				nav_loopTimer 		= millis();
 
 				// calculate the copter's desired bearing and WP distance
 				// ------------------------------------------------------
@@ -1145,8 +1145,12 @@ void update_navigation()
 			// calc a pitch to the target
 			calc_loiter_nav();
 
-		} else {
+			// rotate pitch and roll to the copter frame of reference
+			calc_loiter_output();
 
+		} else {
+			// how far are we from the ideal trajectory?
+			// this pushes us back on course
 			update_crosstrack();
 
 			// calc a rate dampened pitch to the target
@@ -1156,8 +1160,6 @@ void update_navigation()
 			calc_nav_output();
 		}
 
-		//limit our copter pitch - this will change if we go to a fully rate limited approach.
-		limit_nav_pitch_roll(g.pitch_max.get());
 
 		// this tracks a location so the copter is always pointing towards it.
 		if(yaw_tracking == MAV_ROI_LOCATION){
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 953ba5f5cd..fdae032b69 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -390,16 +390,16 @@
 #endif
 
 #ifndef NAV_WP_P
-# define NAV_WP_P				4.0
+# define NAV_WP_P				3.0			// for 4.5 ms error = 13.5 pitch
 #endif
 #ifndef NAV_WP_I
-# define NAV_WP_I				0.15			// leave 0
+# define NAV_WP_I				0.5			// this is a fast ramp up
 #endif
 #ifndef NAV_WP_D
-# define NAV_WP_D				10			// not sure about at all
+# define NAV_WP_D				.1			// slight dampening of a few degrees at most
 #endif
 #ifndef NAV_WP_IMAX
-# define NAV_WP_IMAX			20			// 20 degrees
+# define NAV_WP_IMAX			40			// degrees
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
diff --git a/ArduCopterMega/motors_hexa_p.pde b/ArduCopterMega/motors_hexa_p.pde
index 47db1e17c3..492aa00aed 100644
--- a/ArduCopterMega/motors_hexa_p.pde
+++ b/ArduCopterMega/motors_hexa_p.pde
@@ -84,7 +84,6 @@ void output_motors_disarmed()
 	APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
 }
 
-
 void output_motor_test()
 {
 	APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index b303bdc1e2..f1c4dcc710 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -67,13 +67,14 @@ void calc_loiter_nav()
 	lat_error	= constrain(lat_error,  -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
 
 	// Convert distance into ROLL X
-	//nav_lon		= long_error * g.pid_nav_lon.kP();						// 1800 * 2 = 3600 or 36°
 	nav_lon		= g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
 
 	// PITCH	Y
-	//nav_lat		= lat_error * g.pid_nav_lat.kP();						// 1800 * 2 = 3600 or 36°
 	nav_lat 	= g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);			// invert lat (for pitch)
+}
 
+void calc_loiter_output()
+{
 	// rotate the vector
 	nav_roll 	=   (float)nav_lon * sin_yaw_y 	- (float)nav_lat * -cos_yaw_x;
 					// BAD
@@ -101,6 +102,12 @@ void calc_loiter_nav()
 					//EAST   -1000 * -1			+ 1000 *  0 	=  1000	// pitch back
 					//SOUTH  -1000 *  0			+ 1000 * -1 	= -1000	// pitch forward
 
+	//limit our copter pitch - this will change if we go to a fully rate limited approach.
+
+	long pmax 	= g.pitch_max.get();
+	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
+	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
+	//limit_nav_pitch_roll(g.pitch_max.get());
 }
 
 void calc_simple_nav()
@@ -154,26 +161,18 @@ void calc_rate_nav()
 	long target_error 	= target_bearing - g_gps->ground_course;
 	target_error 		= wrap_180(target_error);
 
-	// calc the cos of the error to tell how fast we are moving towards the target
+	// calc the cos of the error to tell how fast we are moving towards the target in cm
 	int groundspeed 	= (float)g_gps->ground_speed * cos(radians((float)target_error/100));
 
 	// change to rate error
 	// we want to be going 450cm/s
-	int nav_lat 		= WAYPOINT_SPEED - groundspeed;
-	nav_lat 			= constrain(nav_lat, -1800, 1800);
+	int error 			= constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000);
 
 	// Scale response by kP
-	long nav_lat 		= g.pid_nav_wp.kP() * nav_lat;
-	int dampening		= g.pid_nav_wp.kD() * (groundspeed - last_ground_speed);
-
-	// remember our old speed
-	last_ground_speed	= groundspeed;
-
-	// dampen our response
-	nav_lat -= dampening; // +- max error
+	long nav_lat 		= g.pid_nav_wp.get_pid(error, dTnav, 1.0);
 
 	// limit our output
-	nav_lat	= constrain(nav_lat, 	-1800, 1800); // +- max error
+	nav_lat	= constrain(nav_lat, 	-4000, 4000); // +- max error
 }
 
 #endif