diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ad6f19c6e4..cdc503b89c 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -30,6 +30,8 @@ Jack Dunkle			:Alpha testing
 Christof Schmid		:Alpha testing
 Oliver				:Piezo support
 Guntars				:Arming safety suggestion
+Igor van Airde 		:Control Law optimization
+Jean-Louis Naudin 	:Auto Landing
 
 And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
 
@@ -1919,8 +1921,8 @@ static void tuning(){
 
 		case CH6_STABILIZE_KP:
 			g.rc_6.set_range(0,8000); 		// 0 to 8
-			g.pid_rate_roll.kP(tuning_value);
-			g.pid_rate_pitch.kP(tuning_value);
+			g.pi_stabilize_roll.kP(tuning_value);
+			g.pi_stabilize_pitch.kP(tuning_value);
 			break;
 
 		case CH6_STABILIZE_KI:
@@ -1980,11 +1982,17 @@ static void tuning(){
 			break;
 
 		case CH6_NAV_P:
-			g.rc_6.set_range(0,6000);
+			g.rc_6.set_range(0,4000);
 			g.pid_nav_lat.kP(tuning_value);
 			g.pid_nav_lon.kP(tuning_value);
 			break;
 
+		case CH6_NAV_I:
+			g.rc_6.set_range(0,500);
+			g.pid_nav_lat.kI(tuning_value);
+			g.pid_nav_lon.kI(tuning_value);
+			break;
+
 		#if FRAME_CONFIG == HELI_FRAME
 		case CH6_HELI_EXTERNAL_GYRO:
 			g.rc_6.set_range(1000,2000);
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index d7c1c75c70..26e12f3610 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -130,7 +130,6 @@ get_rate_yaw(int32_t target_rate)
 	return constrain(target_rate, -yaw_limit, yaw_limit);
 }
 
-#define ALT_ERROR_MAX 400
 static int16_t
 get_nav_throttle(int32_t z_error)
 {
@@ -138,11 +137,12 @@ get_nav_throttle(int32_t z_error)
 	int16_t rate_error = 0;
 	int16_t output = 0;
 
-	// limit error to prevent I term run up
-	z_error 		= constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
-
 	// convert to desired Rate:
-	rate_error 	= g.pi_alt_hold.get_p(z_error); //_p = .85
+	rate_error 		= g.pi_alt_hold.get_p(z_error);
+	rate_error 		= constrain(rate_error, -100, 100);
+
+	// limit error to prevent I term wind up
+	z_error 		= constrain(z_error, -400, 400);
 
 	// compensates throttle setpoint error for hovering
 	int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde
index 9b1511c07e..0a9fe23cab 100644
--- a/ArduCopter/Log.pde
+++ b/ArduCopter/Log.pde
@@ -535,8 +535,8 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteInt(nav_lat);							// 6
 	DataFlash.WriteInt(x_actual_speed);						// 7
 	DataFlash.WriteInt(y_actual_speed);					    // 8
-	DataFlash.WriteInt(g.pi_loiter_lon.get_integrator());	// 9
-	DataFlash.WriteInt(g.pi_loiter_lat.get_integrator());	// 10
+	DataFlash.WriteInt(g.pid_nav_lon.get_integrator());	// 9
+	DataFlash.WriteInt(g.pid_nav_lat.get_integrator());	// 10
 
 	/*DataFlash.WriteInt(wp_distance);						// 1
 	DataFlash.WriteInt(nav_bearing/100);					// 2
@@ -577,20 +577,17 @@ static void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 
-	DataFlash.WriteInt(g.rc_1.control_in);				// 0
-	DataFlash.WriteInt(g.rc_2.control_in);				// 1
-	DataFlash.WriteInt(g.rc_3.control_in);				// 2
-	DataFlash.WriteInt(g.rc_4.control_in);				// 3
-	DataFlash.WriteInt(sonar_alt);						// 4
-	DataFlash.WriteInt(baro_alt);						// 5
-	DataFlash.WriteInt(next_WP.alt);					// 6
-	DataFlash.WriteInt(nav_throttle);					// 7
-	DataFlash.WriteInt(angle_boost);					// 8
-	DataFlash.WriteInt(manual_boost);					// 9
-	DataFlash.WriteInt(climb_rate);						// 10
-	DataFlash.WriteInt(g.rc_3.servo_out);				// 11
-	DataFlash.WriteInt(g.pi_alt_hold.get_integrator());	// 12
-	DataFlash.WriteInt(g.pid_throttle.get_integrator());	// 13
+	DataFlash.WriteInt(g.rc_3.control_in);				// 1
+	DataFlash.WriteInt(sonar_alt);						// 2
+	DataFlash.WriteInt(baro_alt);						// 3
+	DataFlash.WriteInt(next_WP.alt);					// 4
+	DataFlash.WriteInt(nav_throttle);					// 5
+	DataFlash.WriteInt(angle_boost);					// 6
+	DataFlash.WriteInt(manual_boost);					// 7
+	DataFlash.WriteInt(climb_rate);						// 8
+	DataFlash.WriteInt(g.rc_3.servo_out);				// 9
+	DataFlash.WriteInt(g.pi_alt_hold.get_integrator());	// 10
+	DataFlash.WriteInt(g.pid_throttle.get_integrator());// 11
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -602,7 +599,7 @@ static void Log_Read_Control_Tuning()
 
 	Serial.printf_P(PSTR("CTUN, "));
 
-	for(int8_t i = 0; i < 13; i++ ){
+	for(int8_t i = 0; i < 11; i++ ){
 		temp = DataFlash.ReadInt();
 		Serial.printf("%d, ", temp);
 	}
@@ -696,13 +693,12 @@ static void Log_Write_Attitude()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_ATTITUDE_MSG);
 
+	DataFlash.WriteInt(g.rc_1.control_in);			// 0
 	DataFlash.WriteInt((int)dcm.roll_sensor);		// 1
-	DataFlash.WriteInt((int)dcm.pitch_sensor);		// 2
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);	// 3
-
-	DataFlash.WriteInt((int)g.rc_1.servo_out);		// 4
-	DataFlash.WriteInt((int)g.rc_2.servo_out);		// 5
-	DataFlash.WriteInt((int)g.rc_4.servo_out);		// 6
+	DataFlash.WriteInt(g.rc_2.control_in);			// 2
+	DataFlash.WriteInt((int)dcm.pitch_sensor);		// 3
+	DataFlash.WriteInt(g.rc_4.control_in);			// 4
+	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);	// 5
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -712,10 +708,10 @@ static void Log_Read_Attitude()
 {
 	int16_t temp1 	= DataFlash.ReadInt();
 	int16_t temp2 	= DataFlash.ReadInt();
-	uint16_t temp3 	= DataFlash.ReadInt();
+	int16_t temp3 	= DataFlash.ReadInt();
 	int16_t temp4 	= DataFlash.ReadInt();
 	int16_t temp5 	= DataFlash.ReadInt();
-	int16_t temp6 	= DataFlash.ReadInt();
+	uint16_t temp6 	= DataFlash.ReadInt();
 
 							// 1   2   3    4   5   6
 	Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 9d356f9877..b6e717cee9 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -413,7 +413,6 @@ public:
 	pid_optflow_roll	(k_param_pid_optflow_roll,		PSTR("OF_RLL_"),	OPTFLOW_ROLL_P,		OPTFLOW_ROLL_I,		OPTFLOW_ROLL_D,	OPTFLOW_IMAX * 100),
 	pid_optflow_pitch	(k_param_pid_optflow_pitch,		PSTR("OF_PIT_"),	OPTFLOW_PITCH_P,	OPTFLOW_PITCH_I,	OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100),
 
-
 	// PI controller	group key						name				initial P			initial I			initial imax
 	//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
 	pi_stabilize_roll	(k_param_pi_stabilize_roll,		PSTR("STB_RLL_"),	STABILIZE_ROLL_P,	STABILIZE_ROLL_I,	STABILIZE_ROLL_IMAX * 100),
@@ -424,7 +423,6 @@ public:
 	pi_loiter_lat		(k_param_pi_loiter_lat,			PSTR("HLD_LAT_"),	LOITER_P,			LOITER_I,			LOITER_IMAX * 100),
 	pi_loiter_lon		(k_param_pi_loiter_lon,			PSTR("HLD_LON_"),	LOITER_P,			LOITER_I,			LOITER_IMAX * 100),
 
-
 	junk(0)		// XXX just so that we can add things without worrying about the trailing comma
 	{
 	}
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index cbe6cab5e3..43abc0c984 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -523,21 +523,21 @@
 // Extra motor values that are changed from time to time by jani @ jDrones as software
 // and charachteristics changes.
 #ifdef MOTORS_JD880
-# define STABILIZE_ROLL_P 		3.6
+# define STABILIZE_ROLL_P 		3.7
 # define STABILIZE_ROLL_I 		0.0
-# define STABILIZE_ROLL_IMAX 	40.0		// degrees
-# define STABILIZE_PITCH_P		3.6
+# define STABILIZE_ROLL_IMAX 	        40.0		// degrees
+# define STABILIZE_PITCH_P		3.7
 # define STABILIZE_PITCH_I		0.0
-# define STABILIZE_PITCH_IMAX	40.0		// degrees
+# define STABILIZE_PITCH_IMAX	        40.0		// degrees
 #endif
 
 #ifdef MOTORS_JD850
-# define STABILIZE_ROLL_P 		4.0
+# define STABILIZE_ROLL_P 		4.2
 # define STABILIZE_ROLL_I 		0.0
-# define STABILIZE_ROLL_IMAX 	40.0		// degrees
-# define STABILIZE_PITCH_P		4.0
+# define STABILIZE_ROLL_IMAX 	        40.0		// degrees
+# define STABILIZE_PITCH_P		4.2
 # define STABILIZE_PITCH_I		0.0
-# define STABILIZE_PITCH_IMAX	40.0		// degrees
+# define STABILIZE_PITCH_IMAX	        40.0		// degrees
 #endif
 
 
@@ -587,7 +587,7 @@
 # define RATE_ROLL_I        0.0
 #endif
 #ifndef RATE_ROLL_D
-# define RATE_ROLL_D        0
+# define RATE_ROLL_D        0.0
 #endif
 #ifndef RATE_ROLL_IMAX
 # define RATE_ROLL_IMAX	 	15			// degrees
@@ -597,10 +597,10 @@
 # define RATE_PITCH_P       0.14
 #endif
 #ifndef RATE_PITCH_I
-# define RATE_PITCH_I		0 // 0.18
+# define RATE_PITCH_I		0.0 // 0.18
 #endif
 #ifndef RATE_PITCH_D
-# define RATE_PITCH_D       0 // 0.002
+# define RATE_PITCH_D       0.0 // 0.002
 #endif
 #ifndef RATE_PITCH_IMAX
 # define RATE_PITCH_IMAX   	15			// degrees
@@ -624,10 +624,10 @@
 // Loiter control gains
 //
 #ifndef LOITER_P
-# define LOITER_P			.4		// was .25 in previous
+# define LOITER_P			.2		// was .25 in previous
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.2	// Wind control
+# define LOITER_I			0.0
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees
@@ -640,10 +640,10 @@
 # define NAV_P				2.3			// 3 was causing rapid oscillations in Loiter
 #endif
 #ifndef NAV_I
-# define NAV_I				0			//
+# define NAV_I				0.4			// Wind control
 #endif
 #ifndef NAV_D
-# define NAV_D				0.015		//
+# define NAV_D				0.00		//
 #endif
 #ifndef NAV_IMAX
 # define NAV_IMAX			30			// degrees
@@ -767,7 +767,7 @@
 #endif
 // guess!
 #ifndef LOG_OPTFLOW
-# define LOG_OPTFLOW				DISABLED
+# define LOG_OPTFLOW			DISABLED
 #endif
 
 // calculate the default log_bitmask
diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index 87965160eb..abce5b3c0c 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -77,7 +77,7 @@ static void read_trim_switch()
 			adc.filter_result = false;
 		}
 
-		#elif CH7_OPTION == CH7_AUTO_TRIM
+	#elif CH7_OPTION == CH7_AUTO_TRIM
 		if (g.rc_7.control_in > 800){
 			auto_level_counter = 10;
 		}
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 9ca248da84..d4601ec17f 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -166,6 +166,8 @@
 #define CH6_OPTFLOW_KI 18
 #define CH6_OPTFLOW_KD 19
 
+#define CH6_NAV_I 20
+
 
 // nav byte mask
 // -------------
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index a0eb0e00d7..bd0ef4730c 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -44,8 +44,8 @@ static void calc_XY_velocity(){
 	static int32_t last_longitude = 0;
 	static int32_t last_latitude  = 0;
 
-	//static int16_t x_speed_old = 0;
-	//static int16_t y_speed_old = 0;
+	static int16_t x_speed_old = 0;
+	static int16_t y_speed_old = 0;
 
 	// y_GPS_speed positve = Up
 	// x_GPS_speed positve = Right
@@ -55,20 +55,22 @@ static void calc_XY_velocity(){
 
 	// straightforward approach:
 	///*
-	int16_t	x_estimate	= (float)(g_gps->longitude - last_longitude) * tmp;
-	int16_t	y_estimate	= (float)(g_gps->latitude  - last_latitude)  * tmp;
+	x_actual_speed	= x_speed_old + (float)(g_gps->longitude - last_longitude) * tmp;
+	y_actual_speed	= y_speed_old + (float)(g_gps->latitude  - last_latitude)  * tmp;
 
-	// slight averaging filter
-	x_actual_speed = (x_actual_speed  + x_estimate) >> 1;
-	y_actual_speed = (y_actual_speed  + y_estimate) >> 1;
+	x_actual_speed = x_actual_speed >> 1;
+	y_actual_speed = y_actual_speed >> 1;
+
+	x_speed_old 	= x_actual_speed;
+	y_speed_old 	= y_actual_speed;
 
 	/*
 	// Ryan Beall's forward estimator:
 	int16_t	x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
 	int16_t	y_speed_new = (float)(g_gps->latitude  - last_latitude)  * tmp;
 
-	x_GPS_speed 	= x_speed_new + (x_speed_new - x_speed_old);
-	y_GPS_speed 	= y_speed_new + (y_speed_new - y_speed_old);
+	x_actual_speed 	= x_speed_new + (x_speed_new - x_speed_old);
+	y_actual_speed 	= y_speed_new + (y_speed_new - y_speed_old);
 
 	x_speed_old 	= x_speed_new;
 	y_speed_old 	= y_speed_new;
@@ -96,18 +98,22 @@ static void calc_location_error(struct Location *next_loc)
 	lat_error	= next_loc->lat - current_loc.lat;							// 500 - 0 = 500 Go North
 }
 
-#define NAV_ERR_MAX 800
+#define NAV_ERR_MAX 600
 static void calc_loiter(int x_error, int y_error)
 {
 	// East/West
-	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);	//800
 	int16_t x_target_speed 	= g.pi_loiter_lon.get_p(x_error);
+	x_target_speed			= constrain(x_error, -150, 150);
+	// limit windup
+	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 	int16_t x_iterm 		= g.pi_loiter_lon.get_i(x_error, dTnav);
 	x_rate_error 			= x_target_speed - x_actual_speed;
 
 	// North/South
-	y_error 				= constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 	int16_t y_target_speed 	= g.pi_loiter_lat.get_p(y_error);
+	y_target_speed			= constrain(y_error, -150, 150);
+	// limit windup
+	y_error 				= constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 	int16_t y_iterm 		= g.pi_loiter_lat.get_i(y_error, dTnav);
 	y_rate_error 			= y_target_speed - y_actual_speed;
 
@@ -212,7 +218,7 @@ static void calc_nav_lon(int rate)
 
 static void calc_nav_lat(int rate)
 {
-	nav_lat		= g.pid_nav_lon.get_pid(rate, dTnav);
+	nav_lat		= g.pid_nav_lat.get_pid(rate, dTnav);
 	nav_lat		= get_corrected_angle(rate, nav_lat);
 	nav_lat		= constrain(nav_lat, -3000, 3000);
 }
@@ -396,6 +402,9 @@ static int32_t get_new_altitude()
 		}
 	}
 
+	// we use the elapsed time as our altitude offset
+	// 1000 = 1 sec
+	// 1000 >> 4 = 64cm/s descent by default
 	int32_t change = (millis() - alt_change_timer) >> _scale;
 
 	if(alt_change_flag == ASCENDING){