diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 9628eb8d4c..721d3e3938 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -334,6 +334,9 @@ static int waypoint_speed_gov;
 static bool do_flip = false;
 #endif
 
+static boolean trim_flag;
+static int CH7_wp_index = 0;
+
 // Airspeed
 // --------
 static int		airspeed;							// m/s * 100
@@ -342,6 +345,7 @@ static int		airspeed;							// m/s * 100
 // ---------------
 static long		altitude_error;						// meters * 100 we are off in altitude
 static long 	old_altitude;
+static int 		old_rate;
 static long 	yaw_error;							// how off are we pointed
 static long		long_error, lat_error;				// temp for debugging
 
@@ -381,6 +385,8 @@ static boolean	land_complete;
 static long 	old_alt;							// used for managing altitude rates
 static int		velocity_land;
 static byte 	yaw_tracking = MAV_ROI_WPNEXT;		// no tracking, point at next wp, or at a target
+static int 		manual_boost;						// used in adjust altitude to make changing alt faster
+static int 		angle_boost;						// used in adjust altitude to make changing alt faster
 
 // Loiter management
 // -----------------
@@ -1022,7 +1028,8 @@ void update_throttle_mode(void)
 
 		case THROTTLE_MANUAL:
 			if (g.rc_3.control_in > 0){
-				g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in);
+				angle_boost = get_angle_boost(g.rc_3.control_in);
+				g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
 			}else{
 				g.pi_stabilize_roll.reset_I();
 				g.pi_stabilize_pitch.reset_I();
@@ -1051,8 +1058,8 @@ void update_throttle_mode(void)
 				// clear the new data flag
 				invalid_throttle = false;
 			}
-
-			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise);
+			angle_boost = get_angle_boost(g.throttle_cruise);
+			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
 			break;
 	}
 }
@@ -1213,7 +1220,19 @@ static void update_altitude()
 		current_loc.alt = baro_alt + home.alt;
 	}
 
+	// calc the accel rate limit to 2m/s
 	altitude_rate 	= (current_loc.alt - old_altitude) * 10; // 10 hz timer
+
+	// rate limiter to reduce some of the motor pulsing
+	if (altitude_rate > 0){
+		// going up
+		altitude_rate = min(altitude_rate, old_rate + 20);
+	}else{
+		// going down
+		altitude_rate = max(altitude_rate, old_rate - 20);
+	}
+
+	old_rate 		= altitude_rate;
 	old_altitude 	= current_loc.alt;
 	#endif
 }
@@ -1225,9 +1244,12 @@ adjust_altitude()
 		next_WP.alt -= 1;												// 1 meter per second
 		next_WP.alt = max(next_WP.alt, (current_loc.alt - 500));		// don't go less than 4 meters below current location
 		next_WP.alt = max(next_WP.alt, 100);							// don't go less than 1 meter
+		//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
+
 	}else if (g.rc_3.control_in > 700){
 		next_WP.alt += 1;												// 1 meter per second
 		next_WP.alt = min(next_WP.alt, (current_loc.alt + 500));		// don't go more than 4 meters below current location
+		//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
 	}
 }
 
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index e7b232b359..772828c104 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -83,7 +83,7 @@ get_stabilize_yaw(long target_angle)
 	return (int)constrain(rate, -2500, 2500);
 }
 
-#define ALT_ERROR_MAX 400
+#define ALT_ERROR_MAX 500
 static int
 get_nav_throttle(long z_error)
 {
@@ -94,33 +94,10 @@ get_nav_throttle(long z_error)
 	rate_error 		= rate_error - altitude_rate;
 
 	// limit the rate
-	rate_error 		= constrain(rate_error, -100, 120);
+	rate_error 		= constrain(rate_error, -120, 140);
 	return (int)g.pi_throttle.get_pi(rate_error, .1);
 }
 
-#define ALT_ERROR_MAX2 300
-static int
-get_nav_throttle2(long z_error)
-{
-	if (z_error > ALT_ERROR_MAX2){
-		return g.pi_throttle.kP() * 80;
-
-	}else if (z_error < -ALT_ERROR_MAX2){
-		return g.pi_throttle.kP() * -60;
-
-	} else{
-		// limit error to prevent I term run up
-		z_error 		= constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2);
-		int rate_error 	= g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
-
-		rate_error 		= rate_error - altitude_rate;
-
-		// limit the rate
-		rate_error 		= constrain(rate_error, -100, 120);
-		return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity();
-	}
-}
-
 static int
 get_rate_roll(long target_rate)
 {
diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde
index ba7d02a9a0..7a92352510 100644
--- a/ArduCopter/Log.pde
+++ b/ArduCopter/Log.pde
@@ -680,19 +680,23 @@ static void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 
-
 	// yaw
-	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));			//2
-	DataFlash.WriteInt((int)(nav_yaw/100));					//2
-	DataFlash.WriteInt((int)yaw_error/100);					//3
+	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));				//1
+	DataFlash.WriteInt((int)(nav_yaw/100));						//2
+	DataFlash.WriteInt((int)yaw_error/100);						//3
 
 	// Alt hold
-	DataFlash.WriteInt(g.rc_3.servo_out);					//4
-	DataFlash.WriteInt(sonar_alt);							//5
-	DataFlash.WriteInt(baro_alt);							//6
+	DataFlash.WriteInt(sonar_alt);								//4
+	DataFlash.WriteInt(baro_alt);								//5
+	DataFlash.WriteInt((int)next_WP.alt);						//6
 
-	DataFlash.WriteInt((int)next_WP.alt);					//7
-	DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
+	DataFlash.WriteInt(nav_throttle);							//7
+	DataFlash.WriteInt(angle_boost);							//8
+	DataFlash.WriteByte(manual_boost);							//9
+
+	DataFlash.WriteInt(g.rc_3.servo_out);						//10
+	DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator());	//11
+	DataFlash.WriteInt((int)g.pi_throttle.get_integrator());	//12
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -701,27 +705,30 @@ static void Log_Write_Control_Tuning()
 // Read an control tuning packet
 static void Log_Read_Control_Tuning()
 {
-	Serial.printf_P(PSTR(   "CTUN, "
-							"%d, %d, %d, "
-							"%d, %d, %d, "
-							"%d, %d\n"),
+								//  1   2   3   4   5   6   7   8   9  10  11  12
+	Serial.printf_P(PSTR(   "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
 
 				// Control
 				//DataFlash.ReadByte(),
 				//DataFlash.ReadInt(),
 
 				// yaw
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
+				DataFlash.ReadInt(),	//1
+				DataFlash.ReadInt(),	//2
+				DataFlash.ReadInt(),	//3
 
 				// Alt Hold
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
+				DataFlash.ReadInt(),	//4
+				DataFlash.ReadInt(),	//5
+				DataFlash.ReadInt(),	//6
 
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt());
+				DataFlash.ReadInt(),	//7
+				DataFlash.ReadInt(),	//8
+				DataFlash.ReadByte(),	//9
+
+				DataFlash.ReadInt(),	//10
+				DataFlash.ReadInt(),	//11
+				DataFlash.ReadInt());	//12
 }
 
 // Write a performance monitoring packet. Total length : 19 bytes
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index f0b8e5ee2d..e05c8bb2a8 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -520,7 +520,7 @@
 
 // RATE control
 #ifndef THROTTLE_P
-# define THROTTLE_P		0.6			//
+# define THROTTLE_P		0.8			//
 #endif
 #ifndef THROTTLE_I
 # define THROTTLE_I		0.10		// with 4m error, 12.5s windup
diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index 3d604f612d..632d69c862 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -42,10 +42,6 @@ static void reset_control_switch()
 	read_control_switch();
 }
 
-#if CH7_OPTION == CH7_SET_HOVER
-static boolean trim_flag;
-#endif
-
 // read at 10 hz
 // set this to your trainer switch
 static void read_trim_switch()
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 1979ee328a..c19cc4bdb5 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -348,6 +348,12 @@ static void set_mode(byte mode)
 	// used to stop fly_aways
 	motor_auto_armed = (g.rc_3.control_in > 0);
 
+	// clearing value used in interactive alt hold
+	manual_boost = 0;
+
+	// clearing value used to set WP's dynamically.
+	CH7_wp_index = 0;
+
 	Serial.println(flight_mode_strings[control_mode]);
 
 	// report the GPS and Motor arming status