diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 8df5062431..9a0346ed25 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -959,6 +959,7 @@ void update_navigation()
 	// distance and bearing calcs only
 	if(control_mode == AUTO || control_mode == GCS_AUTO){
 		verify_commands();
+
 	}else{
 		switch(control_mode){
 			case RTL:
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index d4bb8d3711..06c7f6dd4f 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -255,5 +255,8 @@ void output_manual_yaw()
 
 void auto_yaw()
 {
+	if(next_WP.options & WP_OPT_YAW){
+		nav_yaw = target_bearing;
+	}
 	output_yaw_with_hold(true); // hold yaw
 }
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 20aed72e5b..130bb79271 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -101,8 +101,9 @@ void calc_bearing_error()
 void calc_altitude_error()
 {
 	//if(control_mode == AUTO && offset_altitude != 0) {
-	if(next_WP.options & WP_OPT_ALT_CHANGE) {
+	if(next_WP.options & WP_OPT_ALT_CHANGE || offset_altitude == 0) {
 		target_altitude = next_WP.alt;
+
 	}else{
 		// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
 		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - g.waypoint_radius));