diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index ccb7a1bd2d..edf6f4e9d3 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -1151,7 +1151,7 @@ void update_current_flight_mode(void)
 				adjust_altitude();
 
 				// calcualte new nav_yaw offset
-				nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
+				nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle
 
 				// Roll control
 				g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
@@ -1191,7 +1191,7 @@ void update_current_flight_mode(void)
 
 			case LOITER:
 				// calcualte new nav_yaw offset
-				nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
+				nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle
 
 				// allow interactive changing of atitude
 				adjust_altitude();
@@ -1240,34 +1240,34 @@ void update_navigation()
 			update_nav_wp();
 			break;
 
-		case RTL:
-			// calculates desired Yaw
-			update_nav_yaw();
-
 		case GUIDED:
-			update_nav_yaw();
-			// switch passthrough to LOITER
-		case LOITER:
+		case RTL:
+			if(wp_distance > 20){
+				// calculates desired Yaw
+				update_nav_yaw();
+
+			}else{
+				// Don't Yaw anymore
+				// hack to elmininate crosstrack effect
+				crosstrack_bearing 	= target_bearing;
+			}
+
 			// are we Traversing or Loitering?
-			wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE;
-			//wp_control = LOITER_MODE;
+			wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE;
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
 			break;
 
-		/*#if YAW_HACK == 1
-		case SIMPLE:
-			// calculates desired Yaw
-			// exprimental_hack
-			if(g.rc_6.control_in > 900)
-				update_nav_yaw();
-			if(g.rc_6.control_in < 100){
-				nav_yaw = dcm.yaw_sensor;
-			}
+			// switch passthrough to LOITER
+		case LOITER:
+			// are we Traversing or Loitering?
+			//wp_control = (wp_distance < 20) ? LOITER_MODE : WP_MODE;
+			wp_control = LOITER_MODE;
+
+			// calculates the desired Roll and Pitch
+			update_nav_wp();
 			break;
-		#endif
-		*/
 	}
 }
 
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 7d0a9dc7df..5356cfc01e 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -137,11 +137,11 @@ get_throttle(int throttle_input)
 }
 
 long
-get_nav_yaw_offset(int yaw_input, int throttle_input)
+get_nav_yaw_offset(int yaw_input, int reset)
 {
 	long _yaw;
 
-	if(throttle_input == 0){
+	if(reset == 0){
 		// we are on the ground
 		return dcm.yaw_sensor;
 
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 63ef9b203b..3b4d32f527 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -399,13 +399,13 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_P
-# define THROTTLE_P		0.4			// trying a lower val
+# define THROTTLE_P		0.25		// trying a lower val
 #endif
 #ifndef THROTTLE_I
 # define THROTTLE_I		0.01		//with 4m error, 12.5s windup
 #endif
 #ifndef THROTTLE_D
-# define THROTTLE_D		0.35		// upped with filter
+# define THROTTLE_D		0.25		// upped with filter
 #endif
 #ifndef THROTTLE_IMAX
 # define THROTTLE_IMAX		30
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 0b02c2b18c..19507b19c5 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.35 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.36 Beta", main_menu_commands);
 
 void init_ardupilot()
 {