diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 1d1d56fa3f..ef835f8bc1 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -147,18 +147,23 @@ 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_nav_I(void)
+static void reset_hold_I(void)
 {
 	g.pi_loiter_lat.reset_I();
 	g.pi_loiter_lat.reset_I();
 
-	g.pi_nav_lat.reset_I();
-	g.pi_nav_lon.reset_I();
-
 	g.pi_crosstrack.reset_I();
 	g.pi_throttle.reset_I();
 }
 
+// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
+// Keeps outdated data out of our calculations
+static void reset_nav_I(void)
+{
+	g.pi_nav_lat.reset_I();
+	g.pi_nav_lon.reset_I();
+}
+
 
 /*************************************************************
 throttle control
diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde
index 466dd48f68..f92a9a027a 100644
--- a/ArduCopterMega/events.pde
+++ b/ArduCopterMega/events.pde
@@ -33,9 +33,6 @@ static void failsafe_off_event()
 		// --------------------------------------------------------
 		reset_control_switch();
 
-		// Reset control integrators
-		// ---------------------
-		//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 8071583692..bac0fb6da8 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -363,6 +363,7 @@ static void set_mode(byte mode)
 
 	// most modes do not calculate crosstrack correction
 	xtrack_enabled = false;
+	reset_nav_I();
 
 	switch(control_mode)
 	{
@@ -370,21 +371,21 @@ static void set_mode(byte mode)
 			yaw_mode 		= YAW_ACRO;
 			roll_pitch_mode = ROLL_PITCH_ACRO;
 			throttle_mode 	= THROTTLE_MANUAL;
-			reset_nav_I();
+			reset_hold_I();
 			break;
 
 		case STABILIZE:
 			yaw_mode 		= YAW_HOLD;
 			roll_pitch_mode = ROLL_PITCH_STABLE;
 			throttle_mode 	= THROTTLE_MANUAL;
-			reset_nav_I();
+			reset_hold_I();
 			break;
 
 		case SIMPLE:
 			yaw_mode 		= SIMPLE_YAW;
 			roll_pitch_mode = SIMPLE_RP;
 			throttle_mode 	= SIMPLE_THR;
-			reset_nav_I();
+			reset_hold_I();
 			break;
 
 		case ALT_HOLD:
@@ -396,7 +397,7 @@ static void set_mode(byte mode)
 			break;
 
 		case AUTO:
-			reset_nav_I();
+			reset_hold_I();
 			yaw_mode 		= AUTO_YAW;
 			roll_pitch_mode = AUTO_RP;
 			throttle_mode 	= AUTO_THR;