diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index 57e1ac3995..40ca6620c5 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -81,11 +81,9 @@ void increment_WP_index()
 {
 	if(wp_index < wp_total){
 		wp_index++;
-		Serial.print("MSG WP index is incremented to ");
-		Serial.println(wp_index,DEC);
+		Serial.printf_P(PSTR("WP index is incremented to %d\n"),wp_index);
 	}else{
-		Serial.print("MSG Failed to increment WP index of ");
-		Serial.println(wp_index,DEC);
+		Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),wp_index);
 	}
 }
 void decrement_WP_index()
@@ -104,6 +102,9 @@ void decrement_WP_index()
 void
 return_to_launch(void)
 {
+	//so we know where we are navigating from
+	next_WP = current_loc;
+
 	// home is WP 0
 	// ------------
 	wp_index = 0;
@@ -122,9 +123,9 @@ struct
 Location get_LOITER_home_wp()
 {
 	// read home position 
-	struct Location temp = get_wp_with_index(0);	
-	temp.id 	= CMD_LOITER;
-	temp.alt 	= read_alt_to_hold();
+	struct Location temp 	= get_wp_with_index(0);	
+	temp.id 				= CMD_LOITER;
+	temp.alt 				= read_alt_to_hold();
 	return temp;
 }
 
@@ -142,9 +143,7 @@ It looks to see what the next command type is and finds the last command.
 void 
 set_next_WP(struct Location *wp)
 {
-	//send_message(SEVERITY_LOW,"load WP");
-	Serial.print("MSG set_next_WP, wp_index: ");
-	Serial.println(wp_index,DEC);
+	Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), wp_index);
 	send_message(MSG_COMMAND, wp_index);
 
 	// copy the current WP into the OldWP slot
@@ -194,17 +193,14 @@ set_next_WP(struct Location *wp)
 // -------------------------------
 void init_home()
 {
-	Serial.println("MSG: init home");
-
-	// Extra read just in case
-	// -----------------------
-	//GPS.Read();
+	Serial.printf_P(PSTR("init home\n"));
 
 	// block until we get a good fix
 	// -----------------------------
 	while (!GPS.new_data || !GPS.fix) {
 		GPS.update();
 	}
+	
 	home.id 	= CMD_WAYPOINT;
 	home.lng 	= GPS.longitude;				// Lon * 10**7
 	home.lat 	= GPS.latitude;				// Lat * 10**7
@@ -213,8 +209,6 @@ void init_home()
 
 	// ground altitude in centimeters for pressure alt calculations
 	// ------------------------------------------------------------
-	//ground_alt 			= GPS.altitude;
-	//pressure_altitude 	= GPS.altitude;  // Set initial value for filter
 	save_EEPROM_pressure();
 
 	// Save Home to EEPROM
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 62797f3349..b9c01a8b30 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -123,22 +123,21 @@ void calc_distance_error()
 // calculated at 50 hz
 void calc_altitude_error() 
 {
-	altitude_error 		= next_WP.alt - current_loc.alt;
+	if(control_mode == AUTO && offset_altitude != 0) {
+		// limit climb rates - we draw a straight line between first location and edge of wp_radius
+		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - wp_radius));
+		
+		// stay within a certain range
+		if(prev_WP.alt > next_WP.alt){
+			target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
+		}else{
+			target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
+		}
+	} else {
+		target_altitude = next_WP.alt;
+	}
 
-	// limit climb rates
-	//target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
-	
-	//if(prev_WP.alt > next_WP.alt){
-//		target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
-//	}else{
-//		target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
-//	}
-	
-	// calc the GPS/Abs pressure altitude
-	//if(GPS.fix)
-	//	pressure_altitude 	+= altitude_gain * (float)(GPS.altitude - pressure_altitude);
-	//current_loc.alt		= pressure_altitude;
-//	altitude_error 		= target_altitude - current_loc.alt;
+	altitude_error 	= target_altitude - current_loc.alt;
 
 	//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
 }
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 1d829ad7cb..4f2cfc26e7 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -211,6 +211,9 @@ void startup_ground(void)
 		Serial.println("*")
 	}
 	*/
+	// read the radio to set trims
+	// ---------------------------
+	trim_radio();
 
 	if (log_bitmask & MASK_LOG_CMD)
 		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
@@ -228,13 +231,7 @@ void startup_ground(void)
 	
 	//IMU ground start
 	//------------------------
-#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
 	init_pressure_ground();
-#endif
-
-	// read the radio to set trims
-	// ---------------------------
-	trim_radio();
 
 	// Warm up and read Gyro offsets
 	// -----------------------------