diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h
index 915f7143ad..2d04df5661 100644
--- a/ArduCopter/APM_Config.h
+++ b/ArduCopter/APM_Config.h
@@ -52,14 +52,14 @@
 	#define ALT_HOLD_IMAX		300
 
 	// RATE control
-	#define THROTTLE_P			2		//
+	#define THROTTLE_P			3		//
 	#define THROTTLE_I			0.5		// Don't edit
 	#define THROTTLE_D			0.0		//
 
 	#define LOITER_P			0.50
 	#define LOITER_I			0.0
-	#define LOITER_RATE_P		12			//
-	#define LOITER_RATE_I		1.0		// Wind control
+	#define LOITER_RATE_P		5			//
+	#define LOITER_RATE_I		0.1		// Wind control
 	#define LOITER_RATE_D		0.0		// try 2 or 3 for LOITER_RATE 1
 #endif
 
diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde
index 45ebfcc5e9..504381b6fb 100644
--- a/ArduCopter/inertia.pde
+++ b/ArduCopter/inertia.pde
@@ -36,6 +36,11 @@ void z_error_correction()
 	accels_velocity.z	+= speed_error.z * 0.0350;							//speed_correction_z;
 	accels_velocity.z   -= g.pid_throttle.get_integrator() * 0.0045; 		//g.alt_offset_correction; // OK
 	accels_offset.z		-= g.pid_throttle.get_integrator() * 0.000003;		//g.alt_i_correction ; 	// .000002;
+
+	// For developement only
+	// ---------------------
+	if(motors.armed())
+		Log_Write_Raw();
 }
 
 void xy_error_correction()
@@ -62,10 +67,6 @@ void xy_error_correction()
 	accels_offset.y		-= g.pid_loiter_rate_lat.get_integrator() * 0.000003; 	// g.loiter_i_correction;
 
 
-	// For developement only
-	// ---------------------
-	if(motors.armed())
-		Log_Write_Raw();
 }
 
 static void calibrate_accels()
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 9af7209be1..33412c7c15 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -465,6 +465,9 @@ static void set_mode(byte mode)
 			roll_pitch_mode = LOITER_RP;
 			throttle_mode 	= LOITER_THR;
 			set_next_WP(&current_loc);
+			#if INERTIAL_NAV == ENABLED
+			zero_accels();
+			#endif
 			break;
 
 		case POSITION: