diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 7de2de1556..025ee1bc68 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -7,7 +7,7 @@ init_pids()
 	// this creates symmetry with the P gain value preventing oscillations
 
 	max_stabilize_dampener 	= g.pid_stabilize_roll.kP() * 2500;	// = 0.6 * 2500 = 1500 or 15°
-	max_yaw_dampener		= g.pid_yaw.kP() * 6000;				// = .5 * 6000  = 3000
+	//max_yaw_dampener		= g.pid_yaw.kP() * 6000;				// = .35 * 6000  = 2100
 }
 
 void
@@ -57,7 +57,7 @@ output_stabilize_roll()
 
 	// Limit dampening to be equal to propotional term for symmetry
 	rate				= degrees(omega.x) * 100.0; 													// 6rad = 34377
-	dampener 			= (rate * g.stabilize_dampener);										// 34377 * .175 = 6000
+	dampener 			= rate * g.stabilize_dampener;										// 34377 * .175 = 6000
 	g.rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
 }
 
@@ -86,7 +86,7 @@ output_stabilize_pitch()
 
 	// Limit dampening to be equal to propotional term for symmetry
 	rate				= degrees(omega.y) * 100.0; 													// 6rad = 34377
-	dampener 			= (rate * g.stabilize_dampener);										// 34377 * .175 = 6000
+	dampener 			= rate * g.stabilize_dampener;										// 34377 * .175 = 6000
 	g.rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
 }
 
@@ -102,6 +102,11 @@ clear_yaw_control()
 void
 output_yaw_with_hold(boolean hold)
 {
+	// rate control
+	int dampener;
+	long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
+	rate			= constrain(rate, -36000, 36000);									// limit to something fun!
+
 	if(hold){
 		// look to see if we have exited rate control properly - ie stopped turning
 		if(rate_yaw_flag){
@@ -140,33 +145,23 @@ output_yaw_with_hold(boolean hold)
 		g.rc_4.servo_out 	= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .5 * 6000 = 3000
 
 	}else{
-
-		// rate control
-		long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
-		rate			= constrain(rate, -36000, 36000);									// limit to something fun!
-		long error		= ((long)g.rc_4.control_in * 6) - rate;								// control is += 6000 * 6 = 36000
 																							// -error = CCW, 	+error = CW
-
 		if(g.rc_4.control_in == 0){
 			// we are breaking;
 			//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
-			g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
-
+			g.rc_4.servo_out 	= (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
 		}else{
-
-			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
+			long error			= ((long)g.rc_4.control_in * 6) - rate;							// control is += 6000 * 6 = 36000
+			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
 		}
 	}
 
-	// We adjust the output by the rate of rotation
-	long rate			= degrees(omega.z) * 100.0; 									// 3rad = 17188 , 6rad = 34377
-	int dampener 		= (float)rate * g.hold_yaw_dampener;						// 18000 * .17 = 3000
-
 	// Limit dampening to be equal to propotional term for symmetry
-	//g.rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000
+	dampener 			= rate * g.hold_yaw_dampener;											// 34377 * .175 = 6000
+	g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
 
 	// Limit Output
-	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -1800, 1800);							// limit to 24°
+	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -1800, 1800);								// limit to 24°
 
 	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 6c27610f96..c187ff0003 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -336,7 +336,7 @@
 // YAW Control
 //
 #ifndef  YAW_P
-# define YAW_P					0.35		// increase for more aggressive Yaw Hold, decrease if it's bouncy
+# define YAW_P					0.5		// increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
 #ifndef  YAW_I
 # define YAW_I					0.0			// Always 0
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 8b5bff50f8..b0c8aba5b9 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -14,6 +14,11 @@ void arm_motors()
 			if (arming_counter >= ARM_DELAY) {
 				motor_armed 	= true;
 				arming_counter 	= ARM_DELAY;
+
+				// Remember Orientation
+				// ---------------------------
+				init_simple_bearing();
+
 			} else{
 				arming_counter++;
 			}
@@ -244,7 +249,6 @@ set_servos_4()
 			gcs_simple.write_int((int)nav_roll);
 			gcs_simple.write_int((int)nav_pitch);
 
-
 			gcs_simple.write_long(current_loc.lat);	//28
 			gcs_simple.write_long(current_loc.lng);	//32
 			gcs_simple.write_int((int)current_loc.alt);	//34
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 1772be3ab8..ff2abd3b71 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -339,12 +339,10 @@ void set_mode(byte mode)
 
 		case AUTO:
 			init_throttle_cruise();
-			init_simple_bearing();
 			init_auto();
 			break;
 
 		case SIMPLE:
-			init_simple_bearing();
 			break;
 
 		case LOITER:
@@ -487,13 +485,14 @@ unsigned long freeRAM() {
 void
 init_simple_bearing()
 {
-	if(simple_bearing_is_set == false){
+	initial_simple_bearing = dcm.yaw_sensor;
+	//if(simple_bearing_is_set == false){
 		//if(g.rc_3.control_in == 0){
 			// we are on the ground
-			initial_simple_bearing = dcm.yaw_sensor;
-			simple_bearing_is_set = true;
+	//		initial_simple_bearing = dcm.yaw_sensor;
+	//		simple_bearing_is_set = true;
 		//}
-	}
+	//}
 }
 
 void