diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 434368e9c9..fd8d7f5745 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -518,6 +518,11 @@ void fast_loop()
 	if (delta_ms_fast_loop > G_Dt_max)
 		G_Dt_max = delta_ms_fast_loop;
 
+	// Read radio
+	// ----------
+	if (APM_RC.GetState() == 1)
+		read_radio();			// read the radio first
+
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_current_flight_mode();
@@ -538,9 +543,6 @@ void fast_loop()
 
 void medium_loop()
 {
-	// Read radio
-	// ----------
-	read_radio();			// read the radio first
 
 	// reads all of the necessary trig functions for cameras, throttle, etc.
 	update_trig();
@@ -984,6 +986,10 @@ void update_current_flight_mode(void)
 
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
+				// are we at rest? reset nav_yaw
+				if(g.rc_3.control_in == 0){
+					nav_yaw 	= dcm.yaw_sensor;
+				}
 
 				// Yaw control
 				output_manual_yaw();
@@ -1031,6 +1037,11 @@ void update_current_flight_mode(void)
 					limit_nav_pitch_roll(4500);
 				}
 
+				// are we at rest? reset nav_yaw
+				if(g.rc_3.control_in == 0){
+					nav_yaw 	= dcm.yaw_sensor;
+				}
+
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 				// Yaw control
@@ -1052,6 +1063,7 @@ void update_current_flight_mode(void)
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
 
+				// allow interactive changing of atitude
 				adjust_altitude();
 
 				// Yaw control
@@ -1095,7 +1107,7 @@ void update_current_flight_mode(void)
 				break;
 
 			case LOITER:
-
+				// allow interactive changing of atitude
 				adjust_altitude();
 
 				#if AUTO_RESET_LOITER == 1
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 12a6429352..71e54017d2 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -243,7 +243,7 @@ output_yaw_with_hold(boolean hold)
 		yaw_error				= constrain(yaw_error,	 -4000, 4000);						// limit error to 60 degees
 
 		// Apply PID and save the new angle back to RC_Channel
-		g.rc_4.servo_out 		= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
+		g.rc_4.servo_out 		= g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
 
 		// add in yaw dampener
 		g.rc_4.servo_out		-= constrain(dampener, -1600, 1600);
@@ -277,19 +277,15 @@ output_yaw_with_hold(boolean hold)
 void
 output_yaw_with_hold(boolean hold)
 {
-	long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
-	rate			= constrain(rate, -36000, 36000);									// limit to something fun!
-	int dampener 	= rate * g.pid_yaw.kD();											// 34377 * .175 = 6000
-
 	// re-define nav_yaw if we have stick input
 	if(g.rc_4.control_in != 0){
 		// set nav_yaw + or - the current location
 		nav_yaw 	= (long)g.rc_4.control_in + dcm.yaw_sensor;
-
-		// we need to wrap our value so we can be 0 to 360 (*100)
-		nav_yaw 	= wrap_360(nav_yaw);
 	}
 
+	// we need to wrap our value so we can be 0 to 360 (*100)
+	nav_yaw 	= wrap_360(nav_yaw);
+
 	// how far off is nav_yaw from our current yaw?
 	yaw_error 	= nav_yaw - dcm.yaw_sensor;
 
@@ -297,13 +293,13 @@ output_yaw_with_hold(boolean hold)
 	yaw_error 	= wrap_180(yaw_error);
 
 	// limit the error we're feeding to the PID
-	yaw_error				= constrain(yaw_error,	 -4000, 4000);						// limit error to 60 degees
+	yaw_error				= constrain(yaw_error,	 -3500, 3500);						// limit error to 60 degees
 
 	// Apply PID and save the new angle back to RC_Channel
-	g.rc_4.servo_out 		= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
+	g.rc_4.servo_out 		= g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
 
 	// add in yaw dampener
-	g.rc_4.servo_out		-= constrain(dampener, -1600, 1600);
-	yaw_debug 				= YAW_HOLD; //0
+	g.rc_4.servo_out		-= degrees(omega.z) * 100 * g.pid_yaw.kD();
+	yaw_error				= constrain(yaw_error,	 -2500, 2500);						// limit error to 60 degees
 }
 #endif
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 48db1f7b66..cce5251d6a 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -357,13 +357,13 @@
 # define YAW_P					0.4			// increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
 #ifndef  YAW_I
-# define YAW_I					0.0001		// set to .0001 to try and get over user's steady state error caused by poor balance
+# define YAW_I					0.01		// set to .0001 to try and get over user's steady state error caused by poor balance
 #endif
 #ifndef  YAW_D
 # define YAW_D					0.05		// Trying a lower value to prevent odd behavior
 #endif
 #ifndef  YAW_IMAX
-# define YAW_IMAX				.5			// degrees * 100
+# define YAW_IMAX				1			// degrees * 100
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -427,7 +427,7 @@
 # define THROTTLE_BARO_P		0.25
 #endif
 #ifndef THROTTLE_BARO_I
-# define THROTTLE_BARO_I		0.0045
+# define THROTTLE_BARO_I		0.01		//with 4m error, 12.5s windup
 #endif
 #ifndef THROTTLE_BARO_D
 # define THROTTLE_BARO_D		0.03
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 5babaf8aaa..6df3435c40 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.12 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.13 Beta", main_menu_commands);
 
 void init_ardupilot()
 {