diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 456646b55d..1c9c28e469 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -794,7 +794,7 @@ static void fifty_hz_loop()
 	else if (throttle_slew > 0)
 		throttle_slew--;
 
-	throttle_slew = min((800 - throttle), throttle_slew);
+	throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
 
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
@@ -1577,4 +1577,5 @@ static void auto_throttle()
 
 	//Serial.printf("wp:%d, \te:%d \tt%d \t%d\n", (int)next_WP.alt, (int)altitude_error, nav_throttle, g.rc_3.servo_out);
 }
-
+
+
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index b3a709ac85..eec0fcd12f 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -116,8 +116,13 @@ get_rate_yaw(long target_rate)
 // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
 // Keeps outdated data out of our calculations
 static void
-reset_I(void)
+reset_nav_I(void)
 {
+	g.pid_nav_lat.reset_I();
+	g.pid_nav_lon.reset_I();
+	g.pid_nav_wp.reset_I();
+	g.pid_crosstrack.reset_I();
+	g.pid_throttle.reset_I();
 	// I removed these, they don't seem to be needed.
 }
 
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index d3968412fc..e7aa814422 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -376,3 +376,4 @@ public:
 };
 
 #endif // PARAMETERS_H
+
diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde
index 4c7a25a392..466dd48f68 100644
--- a/ArduCopterMega/events.pde
+++ b/ArduCopterMega/events.pde
@@ -35,7 +35,7 @@ static void failsafe_off_event()
 
 		// Reset control integrators
 		// ---------------------
-		reset_I();
+		//reset_nav_I();
 
 	}else if (g.throttle_fs_action == 1){
 		// We're back in radio contact
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index a5e57cd1e5..49fdff3eb2 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -294,13 +294,6 @@ static void init_ardupilot()
 	// -------------------
 	init_commands();
 
-	// Output waypoints for confirmation
-	// XXX do we need this?
-	// --------------------------------
-	//for(int i = 1; i < g.waypoint_total + 1; i++) {
-	//	gcs.send_message(MSG_COMMAND_LIST, i);
-	//}
-
 	// set the correct flight mode
 	// ---------------------------
 	reset_control_switch();
@@ -381,13 +374,13 @@ static void set_mode(byte mode)
 	switch(control_mode)
 	{
 		case ACRO:
-			g.pid_throttle.reset_I();
+			reset_nav_I();
 			break;
 
 		case SIMPLE:
 		case STABILIZE:
 			do_loiter_at_location();
-			g.pid_throttle.reset_I();
+			reset_nav_I();
 			break;
 
 		case ALT_HOLD: