diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index f59fe98a26..ad3dcefb28 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -568,7 +568,11 @@ static int32_t 	ground_pressure;
 static int16_t 	ground_temperature;
 // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
 static int32_t		altitude_error;
-// The cm/s we are moving up or down - Positive = UP
+// The cm/s we are moving up or down based on sensor data - Positive = UP
+static int16_t		climb_rate_actual;
+// Used to dither our climb_rate over 50hz
+static int16_t		climb_rate_error;
+// The cm/s we are moving up or down based on filtered data - Positive = UP
 static int16_t		climb_rate;
 // The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
 static int16_t		sonar_alt;
@@ -578,8 +582,10 @@ static int16_t		sonar_rate;
 static int32_t		baro_alt;
 // The climb_rate as reported by Baro in cm/s
 static int16_t		baro_rate;
-//
+// used to switch out of Manual Boost
 static boolean 		reset_throttle_flag;
+// used to track when to read sensors vs estimate alt
+static boolean 		alt_sensor_flag;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -690,13 +696,6 @@ static int16_t	nav_throttle;						// 0-1000 for throttle control
 // This is a simple counter to track the amount of throttle used during flight
 // This could be useful later in determining and debuging current usage and predicting battery life
 static uint32_t throttle_integrator;
-// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
-// that is generated when the climb rate is within a certain threshold
-//static float	throttle_avg = THROTTLE_CRUISE;
-// This is a flag used to trigger the updating of nav_throttle at 10hz
-static bool 	invalid_throttle;
-// Used to track the altitude offset for climbrate control
-//static int32_t 	target_altitude;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Climb rate control
@@ -1005,8 +1004,9 @@ static void medium_loop()
 					// -----------------------------
 					update_navigation();
 
-					if (g.log_bitmask & MASK_LOG_NTUN)
+					if (g.log_bitmask & MASK_LOG_NTUN && motor_armed){
 						Log_Write_Nav_Tuning();
+					}
 				}
 			}
 			break;
@@ -1018,13 +1018,10 @@ static void medium_loop()
 
 			// Read altitude from sensors
 			// --------------------------
-			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
-			update_altitude();
-			#endif
-
-			// invalidate the throttle hold value
-			// ----------------------------------
-			invalid_throttle = true;
+			//#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
+			//update_altitude();
+			//#endif
+			alt_sensor_flag = true;
 
 			break;
 
@@ -1045,16 +1042,13 @@ static void medium_loop()
                 if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
                     Log_Write_Attitude();
 
-                if (g.log_bitmask & MASK_LOG_CTUN)
-                    Log_Write_Control_Tuning();
+				if (g.log_bitmask & MASK_LOG_MOTORS)
+					Log_Write_Motors();
             }
 
-				// send all requested output streams with rates requested
-				// between 5 and 45 Hz
-				gcs_data_stream_send(5,45);
-
-			if (g.log_bitmask & MASK_LOG_MOTORS)
-				Log_Write_Motors();
+			// send all requested output streams with rates requested
+			// between 5 and 45 Hz
+			gcs_data_stream_send(5,45);
 
 			break;
 
@@ -1102,6 +1096,10 @@ static void medium_loop()
 // ---------------------------
 static void fifty_hz_loop()
 {
+	// read altitude sensors or estimate altitude
+	// ------------------------------------------
+	update_altitude_est();
+
 	// moved to slower loop
 	// --------------------
 	update_throttle_mode();
@@ -1162,6 +1160,11 @@ static void slow_loop()
 			slow_loopCounter++;
 			superslow_loopCounter++;
 
+			// update throttle hold every 20 seconds
+			if(superslow_loopCounter > 60){
+				update_throttle_cruise();
+			}
+
 			if(superslow_loopCounter > 1200){
 				#if HIL_MODE != HIL_MODE_ATTITUDE
 					if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){
@@ -1219,13 +1222,14 @@ static void slow_loop()
 // 1Hz loop
 static void super_slow_loop()
 {
-	if (g.log_bitmask & MASK_LOG_CUR)
+	if (g.log_bitmask & MASK_LOG_CUR && motor_armed)
 		Log_Write_Current();
 
 	// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
 	// but only of the control mode is manual
 	if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
 		auto_disarming_counter++;
+
 		if(auto_disarming_counter == AUTO_ARMING_DELAY){
 			init_disarm_motors();
 		}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
@@ -1234,8 +1238,10 @@ static void super_slow_loop()
 	}else{
 		auto_disarming_counter = 0;
 	}
+
     gcs_send_message(MSG_HEARTBEAT);
     gcs_data_stream_send(1,3);
+
 	// agmatthews - USERHOOKS
 	#ifdef USERHOOK_SUPERSLOWLOOP
 	   USERHOOK_SUPERSLOWLOOP
@@ -1359,12 +1365,13 @@ static void update_GPS(void)
 		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
 		current_loc.lat = g_gps->latitude;	// Lat * 10 * *7
 
-		if (g.log_bitmask & MASK_LOG_GPS){
+		if (g.log_bitmask & MASK_LOG_GPS && motor_armed){
 			Log_Write_GPS();
 		}
 
 		#if HIL_MODE == HIL_MODE_ATTITUDE					// only execute in HIL mode
-			update_altitude();
+			//update_altitude();
+			alt_sensor_flag = true;
 		#endif
 
 	}
@@ -1635,10 +1642,10 @@ void update_throttle_mode(void)
 				if(reset_throttle_flag)	{
 					set_new_altitude(max(current_loc.alt, 100));
 					reset_throttle_flag = false;
+					update_throttle_cruise();
 				}
 
-				// 10hz, 			don't run up i term
-				if(invalid_throttle && motor_auto_armed == true){
+				if(motor_auto_armed == true){
 
 					// how far off are we
 					altitude_error = get_altitude_error();
@@ -1646,8 +1653,6 @@ void update_throttle_mode(void)
 					// get the AP throttle
 					nav_throttle = get_nav_throttle(altitude_error);
 
-					// clear the new data flag
-					invalid_throttle = false;
 					/*
 					Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
 										next_WP.alt,
@@ -1656,6 +1661,7 @@ void update_throttle_mode(void)
 										nav_throttle,
 										(int16_t)g.pi_alt_hold.get_integrator());
 					//*/
+
 				}
 
 				// hack to remove the influence of the ground effect
@@ -1864,7 +1870,6 @@ static void update_altitude()
 {
 	static int16_t 	old_sonar_alt 	= 0;
 	static int32_t 	old_baro_alt 	= 0;
-	static int16_t 	old_climb_rate 	= 0;
 
 	#if HIL_MODE == HIL_MODE_ATTITUDE
 		// we are in the SIM, fake out the baro and Sonar
@@ -1889,7 +1894,6 @@ static void update_altitude()
 		// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
 	#endif
 
-
 	if(g.sonar_enabled){
 		// filter out offset
 		float scale;
@@ -1921,35 +1925,47 @@ static void update_altitude()
 			current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
 
 			// solve for a blended climb_rate
-			climb_rate 		= ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
+			climb_rate_actual = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
 
 		}else{
 			// we must be higher than sonar (>800), don't get tricked by bad sonar reads
 			current_loc.alt = baro_alt + home.alt; // home alt = 0
 			// dont blend, go straight baro
-			climb_rate 		= baro_rate;
+			climb_rate_actual 	= baro_rate;
 		}
 
 	}else{
 		// NO Sonar case
 		current_loc.alt = baro_alt + home.alt;
-		climb_rate 		= baro_rate;
+		climb_rate_actual = baro_rate;
 	}
 
-	// simple smoothing
-	climb_rate = (climb_rate + old_climb_rate)>>1;
-
-	// manage bad data
-	climb_rate = constrain(climb_rate, -800, 800);
-
-	// save for filtering
-	old_climb_rate = climb_rate;
-
 	// update the target altitude
 	next_WP.alt = get_new_altitude();
+
+	// calc error
+	climb_rate_error = (climb_rate_actual - climb_rate) / 5;
 }
 
-#define THROTTLE_ADJUST 250
+static void update_altitude_est()
+{
+	if(alt_sensor_flag){
+		update_altitude();
+		alt_sensor_flag = false;
+
+		if(g.log_bitmask & MASK_LOG_CTUN && motor_armed){
+			Log_Write_Control_Tuning();
+		}
+
+	}else{
+		// simple dithering of climb rate
+		climb_rate += climb_rate_error;
+		current_loc.alt += (climb_rate / 50);
+	}
+	//Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt);
+}
+
+#define THROTTLE_ADJUST 200
 static void
 adjust_altitude()
 {
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index a986a3ccaa..dba7ce508d 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -186,7 +186,7 @@ get_rate_yaw(int32_t target_rate)
 static int16_t
 get_nav_throttle(int32_t z_error)
 {
-	static int16_t old_output = 0;
+	//static int16_t old_output = 0;
 	int16_t rate_error = 0;
 	int16_t output = 0;
 
@@ -198,19 +198,23 @@ get_nav_throttle(int32_t z_error)
 	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);
+	int16_t iterm = g.pi_alt_hold.get_i(z_error, .02);
 
 	// calculate rate error
 	rate_error 		= rate_error - climb_rate;
 
+	// hack to see if we can smooth out oscillations
+	if(rate_error < 0)
+		rate_error = rate_error >> 1;
+
 	// limit the rate
-	output =  constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180);
+	output =  constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120);
 
 	// light filter of output
-	output = (old_output + output) / 2;
+	//output = (old_output + output) / 2;
 
 	// save our output
-	old_output  = output;
+	//old_output  = output;
 
 	// output control:
 	return output + iterm;
@@ -219,8 +223,6 @@ get_nav_throttle(int32_t z_error)
 // Keeps old data out of our calculation / logs
 static void reset_nav_params(void)
 {
-	// forces us to update nav throttle
-	invalid_throttle 		= true;
 	nav_throttle 			= 0;
 
 	// always start Circle mode at same angle
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 78376add1c..fd5be47917 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -741,7 +741,7 @@
 # define THROTTLE_P			0.45	//
 #endif
 #ifndef THROTTLE_I
-# define THROTTLE_I			0.0		//
+# define THROTTLE_I			0.0		// Don't edit
 #endif
 #ifndef THROTTLE_D
 # define THROTTLE_D			0.02	//
diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index bb820aca74..cbfc5615bd 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -186,9 +186,6 @@ static void auto_trim()
 
 			//Serial.println("Done");
 			auto_level_counter = 0;
-
-			// set TC
-			init_throttle_cruise();
 		}
 	}
 }
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index fdea7be828..a3c43799b4 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -532,16 +532,8 @@ static void set_mode(byte mode)
 		motor_auto_armed = true;
 	}
 
-	if(throttle_mode == THROTTLE_MANUAL){
-		// reset all of the throttle iterms
-		update_throttle_cruise();
-
-		// reset auto_throttle
-		nav_throttle 			= 0;
-	}else {
-		// an automatic throttle
-		init_throttle_cruise();
-	}
+	// called to calculate gain for alt hold
+	update_throttle_cruise();
 
 	if(roll_pitch_mode <= ROLL_PITCH_ACRO){
 		// We are under manual attitude control
@@ -594,18 +586,10 @@ static void update_throttle_cruise()
 		g.throttle_cruise += tmp;
 		reset_throttle_I();
 	}
-}
 
-static void
-init_throttle_cruise()
-{
-#if AUTO_THROTTLE_HOLD == 0
-	// are we moving from manual throttle to auto_throttle?
-	if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
-		reset_throttle_I();
-		g.throttle_cruise.set_and_save(g.rc_3.control_in);
-	}
-#endif
+	// recalc kp
+	g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
+	//Serial.printf("kp:%1.4f\n",kp);
 }
 
 #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED