diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde
index b075cc4bb7..5c0e75cd8e 100644
--- a/ArduCopter/motors_hexa.pde
+++ b/ArduCopter/motors_hexa.pde
@@ -167,54 +167,54 @@ static void output_motor_test()
 	motor_out[MOT_5] = g.rc_3.radio_min;
 	motor_out[MOT_6] = g.rc_3.radio_min;
 
-  bool right = (g.rc_1.control_in > 3000);
-  bool left  = (g.rc_1.control_in < -3000);
-  bool front = (g.rc_2.control_in < -3000);
-  bool back = (g.rc_2.control_in > 3000);
+	bool right 	= (g.rc_1.control_in > 3000);
+	bool left  	= (g.rc_1.control_in < -3000);
+	bool front 	= (g.rc_2.control_in < -3000);
+	bool back 	= (g.rc_2.control_in > 3000);
 
 	if(g.frame_orientation == X_FRAME){
 
-		if ( right && !( front || back ) )
+		if(right && !(front || back))
 			motor_out[MOT_1] += 150; // Right
 
-		if ( left && !(front || back ) )
+		if(left && !(front || back))
 			motor_out[MOT_2] += 150; // Left
 
-		if ( back ) {
-      if ( left )
-			  motor_out[MOT_6] += 150;
-      if ( right )
-			  motor_out[MOT_4] += 150;
+		if(back){
+			if(left)
+				motor_out[MOT_6] += 150;
+			if(right)
+				motor_out[MOT_4] += 150;
 		}
 
-		if ( front ) {
-      if ( left )
-			  motor_out[MOT_3] += 150;
-      if ( right )
-			  motor_out[MOT_5] += 150;
+		if(front){
+			if(left)
+				motor_out[MOT_3] += 150;
+			if(right)
+				motor_out[MOT_5] += 150;
 		}
 
 	} else { /* PLUS_FRAME */
 
-    if ( front && !( left || right ) )
-      motor_out[MOT_1] += 150;
+		if(front && !(left || right))
+			motor_out[MOT_1] += 150;
 
-    if ( back && !( left || right ) )
-      motor_out[MOT_2] += 150;
+		if(back && !(left || right))
+			motor_out[MOT_2] += 150;
 
-    if ( left ) {
-      if ( front )
-        motor_out[MOT_5] += 150;
-      if ( back )
-        motor_out[MOT_3] += 150;
-    }
+		if(left){
+			if(front)
+				motor_out[MOT_5] += 150;
+			if(back)
+				motor_out[MOT_3] += 150;
+		}
 
-    if ( right ) {
-      if ( front )
-        motor_out[MOT_4] += 150;
-      if ( back )
-        motor_out[MOT_6] += 150;
-    }
+		if(right){
+			if(front)
+				motor_out[MOT_4] += 150;
+			if(back)
+				motor_out[MOT_6] += 150;
+		}
 
 	}
 
diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde
index 87cc1cd043..402d60da46 100644
--- a/ArduCopter/motors_octa.pde
+++ b/ArduCopter/motors_octa.pde
@@ -5,21 +5,21 @@
 static void init_motors_out()
 {
 	#if INSTANT_PWM == 0
-    APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
-                                | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
+	APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
+								| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8));
 	#endif
 }
 
 static void motors_output_enable()
 {
-  APM_RC.enable_out(MOT_1);
-  APM_RC.enable_out(MOT_2);
-  APM_RC.enable_out(MOT_3);
-  APM_RC.enable_out(MOT_4);
-  APM_RC.enable_out(MOT_5);
-  APM_RC.enable_out(MOT_6);
-  APM_RC.enable_out(MOT_7);
-  APM_RC.enable_out(MOT_8);
+	APM_RC.enable_out(MOT_1);
+	APM_RC.enable_out(MOT_2);
+	APM_RC.enable_out(MOT_3);
+	APM_RC.enable_out(MOT_4);
+	APM_RC.enable_out(MOT_5);
+	APM_RC.enable_out(MOT_6);
+	APM_RC.enable_out(MOT_7);
+	APM_RC.enable_out(MOT_8);
 }
 
 static void output_motors_armed()
@@ -40,16 +40,16 @@ static void output_motors_armed()
 	g.rc_4.calc_pwm();
 
 	if(g.frame_orientation == X_FRAME){
-		roll_out 	 	= (float)g.rc_1.pwm_out * 0.4;
-		pitch_out 	 	= (float)g.rc_2.pwm_out * 0.4;
+		roll_out 	 		= (float)g.rc_1.pwm_out * 0.4;
+		pitch_out 	 		= (float)g.rc_2.pwm_out * 0.4;
 
 		//Front side
-		motor_out[MOT_1]		= g.rc_3.radio_out + g.rc_2.pwm_out - roll_out;	 // CW	 FRONT RIGHT
+		motor_out[MOT_1]	= g.rc_3.radio_out + g.rc_2.pwm_out - roll_out;	 // CW	 FRONT RIGHT
 		motor_out[MOT_5] 	= g.rc_3.radio_out + g.rc_2.pwm_out + roll_out;	 // CCW	 FRONT LEFT
 
 		//Back side
-		motor_out[MOT_2]		= g.rc_3.radio_out - g.rc_2.pwm_out + roll_out;	 // CW	 BACK LEFT
-		motor_out[MOT_4]		= g.rc_3.radio_out - g.rc_2.pwm_out - roll_out;	 // CCW  BACK RIGHT
+		motor_out[MOT_2]	= g.rc_3.radio_out - g.rc_2.pwm_out + roll_out;	 // CW	 BACK LEFT
+		motor_out[MOT_4]	= g.rc_3.radio_out - g.rc_2.pwm_out - roll_out;	 // CCW	BACK RIGHT
 
 		//Left side
 		motor_out[MOT_7] 	= g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW	 LEFT FRONT
@@ -57,14 +57,14 @@ static void output_motors_armed()
 
 		//Right side
 		motor_out[MOT_8] 	= g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW	 RIGHT BACK
-		motor_out[MOT_3]		= g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW	 RIGHT FRONT
+		motor_out[MOT_3]	= g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW	 RIGHT FRONT
 
 	}else if(g.frame_orientation == PLUS_FRAME){
-		roll_out 		= (float)g.rc_1.pwm_out * 0.71;
-		pitch_out 	 	= (float)g.rc_2.pwm_out * 0.71;
+		roll_out 			= (float)g.rc_1.pwm_out * 0.71;
+		pitch_out 	 		= (float)g.rc_2.pwm_out * 0.71;
 
 		//Front side
-		motor_out[MOT_1]		= g.rc_3.radio_out + g.rc_2.pwm_out;		// CW	FRONT
+		motor_out[MOT_1]	= g.rc_3.radio_out + g.rc_2.pwm_out;		// CW	FRONT
 		motor_out[MOT_3] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW	FRONT RIGHT
 		motor_out[MOT_5] 	= g.rc_3.radio_out + roll_out + pitch_out;	// CCW	FRONT LEFT
 
@@ -75,9 +75,9 @@ static void output_motors_armed()
 		motor_out[MOT_8] 	= g.rc_3.radio_out - g.rc_1.pwm_out;		// CW	RIGHT
 
 		//Back side
-		motor_out[MOT_2]		= g.rc_3.radio_out - g.rc_2.pwm_out;		// CW	BACK
-		motor_out[MOT_4]		= g.rc_3.radio_out - roll_out - pitch_out;	// CCW 	BACK RIGHT
-		motor_out[MOT_6]		= g.rc_3.radio_out + roll_out - pitch_out;	// CCW	BACK LEFT
+		motor_out[MOT_2]	= g.rc_3.radio_out - g.rc_2.pwm_out;		// CW	BACK
+		motor_out[MOT_4]	= g.rc_3.radio_out - roll_out - pitch_out;	// CCW 	BACK RIGHT
+		motor_out[MOT_6]	= g.rc_3.radio_out + roll_out - pitch_out;	// CCW	BACK LEFT
 
 	}else if(g.frame_orientation == V_FRAME){
 
@@ -95,63 +95,63 @@ static void output_motors_armed()
 		pitch_out4 	 	= (float)g.rc_2.pwm_out * 0.98;
 
 		//Front side
-		motor_out[MOT_7]	= g.rc_3.radio_out + g.rc_2.pwm_out - roll_out;		// CW  FRONT RIGHT
+		motor_out[MOT_7]	= g.rc_3.radio_out + g.rc_2.pwm_out - roll_out;		// CW	FRONT RIGHT
 		motor_out[MOT_5] 	= g.rc_3.radio_out + g.rc_2.pwm_out + roll_out;		// CCW FRONT LEFT
 
 		//Left side
-		motor_out[MOT_1] 	= g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; 	// CW  LEFT FRONT
+		motor_out[MOT_1] 	= g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; 	// CW	LEFT FRONT
 		motor_out[MOT_3] 	= g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3;	// CCW LEFT BACK
 
 		//Right side
-		motor_out[MOT_2] 	= g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3;	// CW  RIGHT BACK
+		motor_out[MOT_2] 	= g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3;	// CW	RIGHT BACK
 		motor_out[MOT_6]		= g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2;	// CCW RIGHT FRONT
 
 		//Back side
-		motor_out[MOT_8]	= g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4;	// CW  BACK LEFT
-		motor_out[MOT_4]		= g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4;	// CCW BACK RIGHT
+		motor_out[MOT_8]	= g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4;	// CW	BACK LEFT
+		motor_out[MOT_4]	= g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4;	// CCW BACK RIGHT
 
 	}
 
 	// Yaw
-	motor_out[MOT_3]		+= g.rc_4.pwm_out;	// CCW
-	motor_out[MOT_4]		+= g.rc_4.pwm_out;	// CCW
+	motor_out[MOT_3]	+= g.rc_4.pwm_out;	// CCW
+	motor_out[MOT_4]	+= g.rc_4.pwm_out;	// CCW
 	motor_out[MOT_5] 	+= g.rc_4.pwm_out;	// CCW
 	motor_out[MOT_6] 	+= g.rc_4.pwm_out;	// CCW
 
-	motor_out[MOT_1]		-= g.rc_4.pwm_out;	// CW
-	motor_out[MOT_2]		-= g.rc_4.pwm_out;	// CW
+	motor_out[MOT_1]	-= g.rc_4.pwm_out;	// CW
+	motor_out[MOT_2]	-= g.rc_4.pwm_out;	// CW
 	motor_out[MOT_7]	-= g.rc_4.pwm_out;	// CW
 	motor_out[MOT_8]	-= g.rc_4.pwm_out;	// CW
 
 
 	// TODO add stability patch
-	motor_out[MOT_1]		= min(motor_out[MOT_1], 	out_max);
-	motor_out[MOT_2]		= min(motor_out[MOT_2], 	out_max);
-	motor_out[MOT_3]		= min(motor_out[MOT_3], 	out_max);
-	motor_out[MOT_4]		= min(motor_out[MOT_4], 	out_max);
-	motor_out[MOT_5]		= min(motor_out[MOT_5],  out_max);
-	motor_out[MOT_6]		= min(motor_out[MOT_6],  out_max);
-	motor_out[MOT_7]	= min(motor_out[MOT_7], out_max);
-	motor_out[MOT_8] 	= min(motor_out[MOT_8], out_max);
+	motor_out[MOT_1]		= min(motor_out[MOT_1], out_max);
+	motor_out[MOT_2]		= min(motor_out[MOT_2], out_max);
+	motor_out[MOT_3]		= min(motor_out[MOT_3], out_max);
+	motor_out[MOT_4]		= min(motor_out[MOT_4], out_max);
+	motor_out[MOT_5]		= min(motor_out[MOT_5],	out_max);
+	motor_out[MOT_6]		= min(motor_out[MOT_6],	out_max);
+	motor_out[MOT_7]		= min(motor_out[MOT_7], out_max);
+	motor_out[MOT_8] 		= min(motor_out[MOT_8], out_max);
 
 
 	// limit output so motors don't stop
-	motor_out[MOT_1]		= max(motor_out[MOT_1], 	out_min);
-	motor_out[MOT_2]		= max(motor_out[MOT_2], 	out_min);
-	motor_out[MOT_3]		= max(motor_out[MOT_3], 	out_min);
-	motor_out[MOT_4] 	= max(motor_out[MOT_4], 	out_min);
-	motor_out[MOT_5]		= max(motor_out[MOT_5], 	out_min);
-	motor_out[MOT_6] 	= max(motor_out[MOT_6], 	out_min);
-	motor_out[MOT_7]	= max(motor_out[MOT_7], out_min);
-	motor_out[MOT_8] 	= max(motor_out[MOT_8], out_min);
+	motor_out[MOT_1]		= max(motor_out[MOT_1], out_min);
+	motor_out[MOT_2]		= max(motor_out[MOT_2], out_min);
+	motor_out[MOT_3]		= max(motor_out[MOT_3], out_min);
+	motor_out[MOT_4] 		= max(motor_out[MOT_4], out_min);
+	motor_out[MOT_5]		= max(motor_out[MOT_5], out_min);
+	motor_out[MOT_6]	 	= max(motor_out[MOT_6], out_min);
+	motor_out[MOT_7]		= max(motor_out[MOT_7], out_min);
+	motor_out[MOT_8] 		= max(motor_out[MOT_8], out_min);
 
 
 	#if CUT_MOTORS == ENABLED
 	// if we are not sending a throttle output, we cut the motors
 	if(g.rc_3.servo_out == 0){
-		motor_out[MOT_1]		= g.rc_3.radio_min;
-		motor_out[MOT_2]		= g.rc_3.radio_min;
-		motor_out[MOT_3]		= g.rc_3.radio_min;
+		motor_out[MOT_1]	= g.rc_3.radio_min;
+		motor_out[MOT_2]	= g.rc_3.radio_min;
+		motor_out[MOT_3]	= g.rc_3.radio_min;
 		motor_out[MOT_4] 	= g.rc_3.radio_min;
 		motor_out[MOT_5] 	= g.rc_3.radio_min;
 		motor_out[MOT_6] 	= g.rc_3.radio_min;
@@ -162,8 +162,8 @@ static void output_motors_armed()
 
 	// this filter slows the acceleration of motors vs the deceleration
 	// Idea by Denny Rowland to help with his Yaw issue
-	for(int8_t m = 0; m <= 8; m++ ) {
-    int c = ch_of_mot(m);
+	for(int8_t m = 0; m <= 8; m++){
+		int c = ch_of_mot(m);
 		if(motor_filtered[c] < motor_out[c]){
 			motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
 		}else{
@@ -198,7 +198,7 @@ static void output_motors_disarmed()
 	}
 
 	// fill the motor_out[] array for HIL use
-	for (unsigned char i = 0; i < 11; i++) {
+	for (unsigned char i = 0; i < 11; i++){
 		motor_out[i] = g.rc_3.radio_min;
 	}
 
@@ -216,75 +216,110 @@ static void output_motors_disarmed()
 
 static void output_motor_test()
 {
-	if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
-	{
-		APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
-		delay(1000);
 
-		APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
-		delay(1000);
+		motor_out[MOT_1] = g.rc_3.radio_min;
+		motor_out[MOT_2] = g.rc_3.radio_min;
+		motor_out[MOT_3] = g.rc_3.radio_min;
+		motor_out[MOT_4] = g.rc_3.radio_min;
+		motor_out[MOT_5] = g.rc_3.radio_min;
+		motor_out[MOT_6] = g.rc_3.radio_min;
+		motor_out[MOT_7] = g.rc_3.radio_min;
+		motor_out[MOT_8] = g.rc_3.radio_min;
 
-		APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
-		delay(1000);
 
-		APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
-		delay(1000);
+	if(g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME){
+		 APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
+		 delay(4000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
+		 delay(300);
 	}
 
-	if( g.frame_orientation == V_FRAME )
-	{
-		APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
-		delay(1000);
+	if(g.frame_orientation == V_FRAME){
+		 APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
+		 delay(4000);
+		 APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
-		APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
-		delay(1000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
+		 delay(300);
 	}
+
+	APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
+	APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
+	APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
+	APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
+	APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
+	APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
+	APM_RC.OutputCh(MOT_7, motor_out[MOT_7]);
+	APM_RC.OutputCh(MOT_8, motor_out[MOT_8]);
+
 }
 
 #endif
diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde
index b8eb82a561..17094cf699 100644
--- a/ArduCopter/motors_octa_quad.pde
+++ b/ArduCopter/motors_octa_quad.pde
@@ -5,21 +5,21 @@
 static void init_motors_out()
 {
 	#if INSTANT_PWM == 0
-    APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
-                                | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
+	APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
+								| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8));
 	#endif
 }
 
 static void motors_output_enable()
 {
-  APM_RC.enable_out(MOT_1);
-  APM_RC.enable_out(MOT_2);
-  APM_RC.enable_out(MOT_3);
-  APM_RC.enable_out(MOT_4);
-  APM_RC.enable_out(MOT_5);
-  APM_RC.enable_out(MOT_6);
-  APM_RC.enable_out(MOT_7);
-  APM_RC.enable_out(MOT_8);
+	APM_RC.enable_out(MOT_1);
+	APM_RC.enable_out(MOT_2);
+	APM_RC.enable_out(MOT_3);
+	APM_RC.enable_out(MOT_4);
+	APM_RC.enable_out(MOT_5);
+	APM_RC.enable_out(MOT_6);
+	APM_RC.enable_out(MOT_7);
+	APM_RC.enable_out(MOT_8);
 }
 
 static void output_motors_armed()
@@ -47,63 +47,62 @@ static void output_motors_armed()
 		motor_out[MOT_2]		= ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out);		//	APM2 OUT2	APM1 OUT2	 FRONT LEFT		CW		TOP
 		motor_out[MOT_3]		= ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out);		//	APM2 OUT3	APM1 OUT3	 BACK LEFT		CCW		TOP
 		motor_out[MOT_4]		= ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out);		//	APM2 OUT4	APM1 OUT4	 BACK RIGHT		CW		TOP
-		motor_out[MOT_5]		=  g.rc_3.radio_out + roll_out + pitch_out;								//	APM2 OUT5	APM1 OUT7	 FRONT LEFT		CCW		BOTTOM
-		motor_out[MOT_6]		=  g.rc_3.radio_out - roll_out + pitch_out;								//	APM2 OUT6	APM1 OUT8	 FRONT RIGHT	CW		BOTTOM
-		motor_out[MOT_7]		=  g.rc_3.radio_out - roll_out - pitch_out;								//	APM2 OUT7	APM1 OUT10	 BACK RIGHT		CCW		BOTTOM
-		motor_out[MOT_8]		=  g.rc_3.radio_out + roll_out - pitch_out;								//	APM2 OUT8	APM1 OUT11	 BACK LEFT		CW		BOTTOM
+		motor_out[MOT_5]		=	g.rc_3.radio_out + roll_out + pitch_out;								//	APM2 OUT5	APM1 OUT7	 FRONT LEFT		CCW		BOTTOM
+		motor_out[MOT_6]		=	g.rc_3.radio_out - roll_out + pitch_out;								//	APM2 OUT6	APM1 OUT8	 FRONT RIGHT	CW		BOTTOM
+		motor_out[MOT_7]		=	g.rc_3.radio_out - roll_out - pitch_out;								//	APM2 OUT7	APM1 OUT10	 BACK RIGHT		CCW		BOTTOM
+		motor_out[MOT_8]		=	g.rc_3.radio_out + roll_out - pitch_out;								//	APM2 OUT8	APM1 OUT11	 BACK LEFT		CW		BOTTOM
 
 	}else{
-		roll_out 	 	= g.rc_1.pwm_out;
-		pitch_out 	 	= g.rc_2.pwm_out;
+		roll_out 	 			= g.rc_1.pwm_out;
+		pitch_out 	 			= g.rc_2.pwm_out;
 
-		motor_out[MOT_1]		= (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out;	//APM2 OUT1		APM1 OUT1		FRONT	CCW		TOP
-		motor_out[MOT_2]		= (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out;	//APM2 OUT2		APM1 OUT2		LEFT	CW		TOP
-		motor_out[MOT_3]		= (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out;	//APM2 OUT3		APM1 OUT3		BACK	CCW		TOP
-		motor_out[MOT_4]		= (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out;	//APM2 OUT4		APM1 OUT4		RIGHT	CW		TOP
-		motor_out[MOT_5]		=  g.rc_3.radio_out + roll_out;							//APM2 OUT5		APM1 OUT7		LEFT	CCW		BOTTOM
-		motor_out[MOT_6]		=  g.rc_3.radio_out + pitch_out;						//APM2 OUT6		APM1 OUT8		FRONT	CW		BOTTOM
-		motor_out[MOT_7]		=  g.rc_3.radio_out - roll_out;							//APM2 OUT7		APM1 OUT10		RIGHT	CCW		BOTTOM
-		motor_out[MOT_8]		=  g.rc_3.radio_out - pitch_out;						//APM2 OUT8		APM1 OUT11		BACK	CW		BOTTOM
+		motor_out[MOT_1]		= (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out;	// APM2 OUT1		APM1 OUT1		FRONT	CCW		TOP
+		motor_out[MOT_2]		= (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out;	// APM2 OUT2		APM1 OUT2		LEFT	CW		TOP
+		motor_out[MOT_3]		= (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out;	// APM2 OUT3		APM1 OUT3		BACK	CCW		TOP
+		motor_out[MOT_4]		= (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out;	// APM2 OUT4		APM1 OUT4		RIGHT	CW		TOP
+		motor_out[MOT_5]		= g.rc_3.radio_out + roll_out;							// APM2 OUT5		APM1 OUT7		LEFT	CCW		BOTTOM
+		motor_out[MOT_6]		= g.rc_3.radio_out + pitch_out;						// APM2 OUT6		APM1 OUT8		FRONT	CW		BOTTOM
+		motor_out[MOT_7]		= g.rc_3.radio_out - roll_out;							// APM2 OUT7		APM1 OUT10		RIGHT	CCW		BOTTOM
+		motor_out[MOT_8]		= g.rc_3.radio_out - pitch_out;						// APM2 OUT8		APM1 OUT11		BACK	CW		BOTTOM
 
-	// Yaw
-	motor_out[MOT_1]		+= g.rc_4.pwm_out;	// CCW
-	motor_out[MOT_3]		+= g.rc_4.pwm_out;	// CCW
-	motor_out[MOT_5]		+= g.rc_4.pwm_out;	// CCW
-	motor_out[MOT_7]		+= g.rc_4.pwm_out;	// CCW
+		// Yaw
+		motor_out[MOT_1]		+= g.rc_4.pwm_out;	// CCW
+		motor_out[MOT_3]		+= g.rc_4.pwm_out;	// CCW
+		motor_out[MOT_5]		+= g.rc_4.pwm_out;	// CCW
+		motor_out[MOT_7]		+= g.rc_4.pwm_out;	// CCW
 
-	motor_out[MOT_2]		-= g.rc_4.pwm_out;	// CW
-	motor_out[MOT_4]		-= g.rc_4.pwm_out;	// CW
-	motor_out[MOT_6]		-= g.rc_4.pwm_out;	// CW
-	motor_out[MOT_8]		-= g.rc_4.pwm_out;	// CW
-
-		
+		motor_out[MOT_2]		-= g.rc_4.pwm_out;	// CW
+		motor_out[MOT_4]		-= g.rc_4.pwm_out;	// CW
+		motor_out[MOT_6]		-= g.rc_4.pwm_out;	// CW
+		motor_out[MOT_8]		-= g.rc_4.pwm_out;	// CW
 	}
+
 	// TODO add stability patch
-	motor_out[MOT_1]		= min(motor_out[MOT_1], 	out_max);
-	motor_out[MOT_2]		= min(motor_out[MOT_2], 	out_max);
-	motor_out[MOT_3]		= min(motor_out[MOT_3], 	out_max);
-	motor_out[MOT_4]		= min(motor_out[MOT_4], 	out_max);
-	motor_out[MOT_5]		= min(motor_out[MOT_5],  out_max);
-	motor_out[MOT_6]		= min(motor_out[MOT_6],  out_max);
-	motor_out[MOT_7]	= min(motor_out[MOT_7], out_max);
-	motor_out[MOT_8] 	= min(motor_out[MOT_8], out_max);
+	motor_out[MOT_1]		= min(motor_out[MOT_1], out_max);
+	motor_out[MOT_2]		= min(motor_out[MOT_2], out_max);
+	motor_out[MOT_3]		= min(motor_out[MOT_3], out_max);
+	motor_out[MOT_4]		= min(motor_out[MOT_4], out_max);
+	motor_out[MOT_5]		= min(motor_out[MOT_5],	out_max);
+	motor_out[MOT_6]		= min(motor_out[MOT_6],	out_max);
+	motor_out[MOT_7]		= min(motor_out[MOT_7], out_max);
+	motor_out[MOT_8] 		= min(motor_out[MOT_8], out_max);
 
 	// limit output so motors don't stop
 	motor_out[MOT_1]		= max(motor_out[MOT_1], 	out_min);
 	motor_out[MOT_2]		= max(motor_out[MOT_2], 	out_min);
 	motor_out[MOT_3]		= max(motor_out[MOT_3], 	out_min);
-	motor_out[MOT_4] 	= max(motor_out[MOT_4], 	out_min);
+	motor_out[MOT_4] 		= max(motor_out[MOT_4], 	out_min);
 	motor_out[MOT_5]		= max(motor_out[MOT_5], 	out_min);
-	motor_out[MOT_6] 	= max(motor_out[MOT_6], 	out_min);
-	motor_out[MOT_7]	= max(motor_out[MOT_7], out_min);
-	motor_out[MOT_8] 	= max(motor_out[MOT_8], out_min);
+	motor_out[MOT_6] 		= max(motor_out[MOT_6], 	out_min);
+	motor_out[MOT_7]		= max(motor_out[MOT_7], out_min);
+	motor_out[MOT_8] 		= max(motor_out[MOT_8], out_min);
 
 	#if CUT_MOTORS == ENABLED
 	// if we are not sending a throttle output, we cut the motors
 	if(g.rc_3.servo_out == 0){
-		motor_out[MOT_1]		= g.rc_3.radio_min;
-		motor_out[MOT_2]		= g.rc_3.radio_min;
-		motor_out[MOT_3]		= g.rc_3.radio_min;
+		motor_out[MOT_1]	= g.rc_3.radio_min;
+		motor_out[MOT_2]	= g.rc_3.radio_min;
+		motor_out[MOT_3]	= g.rc_3.radio_min;
 		motor_out[MOT_4] 	= g.rc_3.radio_min;
 		motor_out[MOT_5] 	= g.rc_3.radio_min;
 		motor_out[MOT_6] 	= g.rc_3.radio_min;
@@ -114,8 +113,8 @@ static void output_motors_armed()
 
 	// this filter slows the acceleration of motors vs the deceleration
 	// Idea by Denny Rowland to help with his Yaw issue
-	for(int8_t m = 0; m <= 8; m++ ) {
-    int i = ch_of_mot(m);
+	for(int8_t m = 0; m <= 8; m++){
+		int i = ch_of_mot(m);
 		if(motor_filtered[i] < motor_out[i]){
 			motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
 		}else{
@@ -150,7 +149,7 @@ static void output_motors_disarmed()
 	}
 
 	// fill the motor_out[] array for HIL use
-	for (unsigned char i = 0; i < 11; i++) {
+	for (unsigned char i = 0; i < 11; i++){
 		motor_out[i] = g.rc_3.radio_min;
 	}
 
@@ -167,37 +166,62 @@ static void output_motors_disarmed()
 
 static void output_motor_test()
 {
-	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
-	delay(1000);
-
-	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
-	delay(1000);
+	motor_out[MOT_1] = g.rc_3.radio_min;
+	motor_out[MOT_2] = g.rc_3.radio_min;
+	motor_out[MOT_3] = g.rc_3.radio_min;
+	motor_out[MOT_4] = g.rc_3.radio_min;
+	motor_out[MOT_5] = g.rc_3.radio_min;
+	motor_out[MOT_6] = g.rc_3.radio_min;
+	motor_out[MOT_7] = g.rc_3.radio_min;
+	motor_out[MOT_8] = g.rc_3.radio_min;
 
 	APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
-	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
-	delay(1000);
-}
+	delay(5000);
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+	delay(300);
 
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
+	APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
+	APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
+	APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
+	APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
+	APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
+	APM_RC.OutputCh(MOT_7, motor_out[MOT_7]);
+	APM_RC.OutputCh(MOT_8, motor_out[MOT_8]);
+}
 #endif
diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde
index 54a9c4d6f4..55c21f7b97 100644
--- a/ArduCopter/motors_quad.pde
+++ b/ArduCopter/motors_quad.pde
@@ -5,16 +5,16 @@
 static void init_motors_out()
 {
 	#if INSTANT_PWM == 0
-    APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) );
+	APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4));
 	#endif
 }
 
 static void motors_output_enable()
 {
-  APM_RC.enable_out(MOT_1);
-  APM_RC.enable_out(MOT_2);
-  APM_RC.enable_out(MOT_3);
-  APM_RC.enable_out(MOT_4);
+	APM_RC.enable_out(MOT_1);
+	APM_RC.enable_out(MOT_2);
+	APM_RC.enable_out(MOT_3);
+	APM_RC.enable_out(MOT_4);
 }
 
 static void output_motors_armed()
@@ -41,11 +41,11 @@ static void output_motors_armed()
 		pitch_out 	 	= g.rc_2.pwm_out * .707;
 
 		// left
-		motor_out[MOT_3]	 	= g.rc_3.radio_out + roll_out + pitch_out;	// FRONT
-		motor_out[MOT_2]	 	= g.rc_3.radio_out + roll_out - pitch_out;	// BACK
+		motor_out[MOT_3]	= g.rc_3.radio_out + roll_out + pitch_out;	// FRONT
+		motor_out[MOT_2]	= g.rc_3.radio_out + roll_out - pitch_out;	// BACK
 
 		// right
-		motor_out[MOT_1]		= g.rc_3.radio_out - roll_out + pitch_out;  // FRONT
+		motor_out[MOT_1]	= g.rc_3.radio_out - roll_out + pitch_out;	// FRONT
 		motor_out[MOT_4] 	= g.rc_3.radio_out - roll_out - pitch_out;	// BACK
 
 	}else{
@@ -54,54 +54,54 @@ static void output_motors_armed()
 		pitch_out 	 	= g.rc_2.pwm_out;
 
 		// right motor
-		motor_out[MOT_1]		= g.rc_3.radio_out - roll_out;
+		motor_out[MOT_1]	= g.rc_3.radio_out - roll_out;
 		// left motor
-		motor_out[MOT_2]		= g.rc_3.radio_out + roll_out;
+		motor_out[MOT_2]	= g.rc_3.radio_out + roll_out;
 		// front motor
-		motor_out[MOT_3]		= g.rc_3.radio_out + pitch_out;
+		motor_out[MOT_3]	= g.rc_3.radio_out + pitch_out;
 		// back motor
 		motor_out[MOT_4] 	= g.rc_3.radio_out - pitch_out;
 	}
 
 	// Yaw input
-	motor_out[MOT_1]		+=  g.rc_4.pwm_out; 	// CCW
-	motor_out[MOT_2]		+=  g.rc_4.pwm_out; 	// CCW
-	motor_out[MOT_3]		-=  g.rc_4.pwm_out; 	// CW
-	motor_out[MOT_4] 	-=  g.rc_4.pwm_out; 	// CW
+	motor_out[MOT_1]	+=	g.rc_4.pwm_out; 	// CCW
+	motor_out[MOT_2]	+=	g.rc_4.pwm_out; 	// CCW
+	motor_out[MOT_3]	-=	g.rc_4.pwm_out; 	// CW
+	motor_out[MOT_4] 	-=	g.rc_4.pwm_out; 	// CW
 
     /* We need to clip motor output at out_max. When cipping a motors
-     * output we also need to compensate for the instability by
-     * lowering the opposite motor by the same proportion. This
-     * ensures that we retain control when one or more of the motors
-     * is at its maximum output
-     */
-    for (int i=MOT_1; i<=MOT_4; i++) {
-        if (motor_out[i] > out_max) {
-            // note that i^1 is the opposite motor
-            motor_out[i^1] -= motor_out[i] - out_max;
-            motor_out[i] = out_max;
-        }
-    }
+		 * output we also need to compensate for the instability by
+		 * lowering the opposite motor by the same proportion. This
+		 * ensures that we retain control when one or more of the motors
+		 * is at its maximum output
+		 */
+		for (int i = MOT_1; i <= MOT_4; i++){
+				if(motor_out[i] > out_max){
+		            // note that i^1 is the opposite motor
+					motor_out[i ^ 1] -= motor_out[i] - out_max;
+					motor_out[i] = out_max;
+				}
+		}
 
 	// limit output so motors don't stop
-	motor_out[MOT_1]		= max(motor_out[MOT_1], 	out_min);
-	motor_out[MOT_2]		= max(motor_out[MOT_2], 	out_min);
-	motor_out[MOT_3]		= max(motor_out[MOT_3], 	out_min);
+	motor_out[MOT_1]	= max(motor_out[MOT_1], 	out_min);
+	motor_out[MOT_2]	= max(motor_out[MOT_2], 	out_min);
+	motor_out[MOT_3]	= max(motor_out[MOT_3], 	out_min);
 	motor_out[MOT_4] 	= max(motor_out[MOT_4], 	out_min);
 
 	#if CUT_MOTORS == ENABLED
 	// if we are not sending a throttle output, we cut the motors
 	if(g.rc_3.servo_out == 0){
-		motor_out[MOT_1]		= g.rc_3.radio_min;
-		motor_out[MOT_2]		= g.rc_3.radio_min;
-		motor_out[MOT_3]		= g.rc_3.radio_min;
+		motor_out[MOT_1]	= g.rc_3.radio_min;
+		motor_out[MOT_2]	= g.rc_3.radio_min;
+		motor_out[MOT_3]	= g.rc_3.radio_min;
 		motor_out[MOT_4] 	= g.rc_3.radio_min;
 	}
 	#endif
 
 	// this filter slows the acceleration of motors vs the deceleration
 	// Idea by Denny Rowland to help with his Yaw issue
-	for(int8_t i=MOT_1; i <= MOT_4; i++ ) {
+	for(int8_t i = MOT_1; i <= MOT_4; i++){
 		if(motor_filtered[i] < motor_out[i]){
 			motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
 		}else{
@@ -133,7 +133,7 @@ static void output_motors_disarmed()
 	}
 
 	// fill the motor_out[] array for HIL use
-	for (unsigned char i = 0; i < 8; i++) {
+	for (unsigned char i = 0; i < 8; i++){
 		motor_out[i] = g.rc_3.radio_min;
 	}
 
@@ -164,43 +164,49 @@ static void output_motor_test()
 
 
 	if(g.frame_orientation == X_FRAME){
-//  31
-//	24
-		if(g.rc_1.control_in > 3000){
-			motor_out[MOT_1] += 100;
-			motor_out[MOT_4] += 100;
-		}
 
-		if(g.rc_1.control_in < -3000){
-			motor_out[MOT_2] += 100;
-			motor_out[MOT_3] += 100;
-		}
+		 APM_RC.OutputCh(MOT_3, g.rc_2.radio_min);
+		 delay(4000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		if(g.rc_2.control_in > 3000){
-			motor_out[MOT_2] += 100;
-			motor_out[MOT_4] += 100;
-		}
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_4, g.rc_1.radio_min + 100);
+		 delay(300);
 
-		if(g.rc_2.control_in < -3000){
-			motor_out[MOT_1] += 100;
-			motor_out[MOT_3] += 100;
-		}
+		 APM_RC.OutputCh(MOT_4, g.rc_1.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_2, g.rc_4.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_2, g.rc_4.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_3, g.rc_2.radio_min + 100);
+		 delay(300);
 
 	}else{
-//  3
-// 2 1
-//	4
-		if(g.rc_1.control_in > 3000)
-			motor_out[MOT_1] += 100;
 
-		if(g.rc_1.control_in < -3000)
-			motor_out[MOT_2] += 100;
+		 APM_RC.OutputCh(MOT_3, g.rc_2.radio_min);
+		 delay(4000);
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+		 delay(300);
 
-		if(g.rc_2.control_in > 3000)
-			motor_out[MOT_4] += 100;
+		 APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_2, g.rc_1.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_2, g.rc_1.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_4, g.rc_4.radio_min + 100);
+		 delay(300);
+
+		 APM_RC.OutputCh(MOT_4, g.rc_4.radio_min);
+		 delay(2000);
+		 APM_RC.OutputCh(MOT_3, g.rc_2.radio_min + 100);
+		 delay(300);
 
-		if(g.rc_2.control_in < -3000)
-			motor_out[MOT_3] += 100;
 	}
 
 	APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde
index 3fbdb49e8f..037ea1dc4f 100644
--- a/ArduCopter/motors_tri.pde
+++ b/ArduCopter/motors_tri.pde
@@ -4,16 +4,16 @@
 static void init_motors_out()
 {
 	#if INSTANT_PWM == 0
-    APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4) );
+		APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4));
 	#endif
 }
 
 static void motors_output_enable()
 {
-  APM_RC.enable_out(MOT_1);
-  APM_RC.enable_out(MOT_2);
-  APM_RC.enable_out(MOT_4);
-  APM_RC.enable_out(CH_TRI_YAW);
+	APM_RC.enable_out(MOT_1);
+	APM_RC.enable_out(MOT_2);
+	APM_RC.enable_out(MOT_4);
+	APM_RC.enable_out(CH_TRI_YAW);
 }
 
 
@@ -45,19 +45,19 @@ static void output_motors_armed()
 	//motor_out[MOT_4]		+= (float)(abs(g.rc_4.control_in)) * .013;
 
 	// Tridge's stability patch
-	if (motor_out[MOT_1] > out_max) {
+	if(motor_out[MOT_1] > out_max){
 		motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1;
 		motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1;
 		motor_out[MOT_1] = out_max;
 	}
 
-	if (motor_out[MOT_2] > out_max) {
+	if(motor_out[MOT_2] > out_max){
 		motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1;
 		motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1;
 		motor_out[MOT_2] = out_max;
 	}
 
-	if (motor_out[MOT_4] > out_max) {
+	if(motor_out[MOT_4] > out_max){
 		motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1;
 		motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1;
 		motor_out[MOT_4] = out_max;
@@ -97,7 +97,7 @@ static void output_motors_disarmed()
 	}
 
 	// fill the motor_out[] array for HIL use
-	for (unsigned char i = 0; i < 8; i++) {
+	for (unsigned char i = 0; i < 8; i++){
 		motor_out[i] = g.rc_3.radio_min;
 	}
 
@@ -113,18 +113,20 @@ static void output_motor_test()
 	motor_out[MOT_2] = g.rc_3.radio_min;
 	motor_out[MOT_4] = g.rc_3.radio_min;
 
+	 APM_RC.OutputCh(MOT_2, g.rc_2.radio_min);
+	delay(4000);
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+	delay(300);
 
-	if(g.rc_1.control_in > 3000){	// right
-		motor_out[MOT_1] += 100;
-	}
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+	delay(2000);
+	APM_RC.OutputCh(MOT_4, g.rc_1.radio_min + 100);
+	delay(300);
 
-	if(g.rc_1.control_in < -3000){	// left
-		motor_out[MOT_2] += 100;
-	}
-
-	if(g.rc_2.control_in > 3000){	// back
-		motor_out[MOT_4] += 100;
-	}
+	APM_RC.OutputCh(MOT_4, g.rc_1.radio_min);
+	delay(2000);
+	APM_RC.OutputCh(MOT_2, g.rc_4.radio_min + 100);
+	delay(300);
 
 	APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
 	APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde
index 991ced7e49..c18c854fad 100644
--- a/ArduCopter/motors_y6.pde
+++ b/ArduCopter/motors_y6.pde
@@ -7,19 +7,19 @@
 static void init_motors_out()
 {
 	#if INSTANT_PWM == 0
-    APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
-                                | _BV(MOT_5) | _BV(MOT_6) );
+		APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
+																| _BV(MOT_5) | _BV(MOT_6));
 	#endif
 }
 
 static void motors_output_enable()
 {
-  APM_RC.enable_out(MOT_1);
-  APM_RC.enable_out(MOT_2);
-  APM_RC.enable_out(MOT_3);
-  APM_RC.enable_out(MOT_4);
-  APM_RC.enable_out(MOT_5);
-  APM_RC.enable_out(MOT_6);
+	APM_RC.enable_out(MOT_1);
+	APM_RC.enable_out(MOT_2);
+	APM_RC.enable_out(MOT_3);
+	APM_RC.enable_out(MOT_4);
+	APM_RC.enable_out(MOT_5);
+	APM_RC.enable_out(MOT_6);
 }
 
 static void output_motors_armed()
@@ -40,41 +40,41 @@ static void output_motors_armed()
 
 	// Multi-Wii Mix
 	//left
-	motor_out[MOT_2] 			= (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT	 TOP - CW
-	motor_out[MOT_3] 			= g.rc_3.radio_out + g.rc_1.pwm_out	+ (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
+	motor_out[MOT_2] 		= (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT	 TOP - CW
+	motor_out[MOT_3] 		= g.rc_3.radio_out + g.rc_1.pwm_out	+ (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
 	//right
-	motor_out[MOT_5] 			= (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
-	motor_out[MOT_1] 			= g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
+	motor_out[MOT_5] 		= (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
+	motor_out[MOT_1] 		= g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
 	//back
-	motor_out[MOT_6] 			= (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); 					// REAR TOP - CCW
-	motor_out[MOT_4] 			= g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); 					// BOTTOM_REAR - CW
+	motor_out[MOT_6] 		= (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); 					// REAR TOP - CCW
+	motor_out[MOT_4] 		= g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); 					// BOTTOM_REAR - CW
 
 	//left
-	motor_out[MOT_2] 			-= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
-	motor_out[MOT_3] 			+= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
+	motor_out[MOT_2] 		-= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
+	motor_out[MOT_3] 		+= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
 	//right
-	motor_out[MOT_5] 			-= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
-	motor_out[MOT_1] 			+= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
+	motor_out[MOT_5] 		-= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
+	motor_out[MOT_1] 		+= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
 	//back
-	motor_out[MOT_6] 			+= YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
-	motor_out[MOT_4] 			-= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
+	motor_out[MOT_6] 		+= YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
+	motor_out[MOT_4] 		-= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
 
 
 	/*
 	int roll_out 		= (float)g.rc_1.pwm_out * .866;
-	int pitch_out 		=  g.rc_2.pwm_out / 2;
+	int pitch_out 		=	g.rc_2.pwm_out / 2;
 
 	//left
-	motor_out[MOT_2]		= ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out);  // CCW TOP
-	motor_out[MOT_3]		=  g.rc_3.radio_out + roll_out + pitch_out;							// CW
+	motor_out[MOT_2]		= ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out);	// CCW TOP
+	motor_out[MOT_3]		=	g.rc_3.radio_out + roll_out + pitch_out;							// CW
 
 	//right
 	motor_out[MOT_5]		= ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out);	// CCW TOP
-	motor_out[MOT_1]		=  g.rc_3.radio_out - roll_out + pitch_out;							// CW
+	motor_out[MOT_1]		=	g.rc_3.radio_out - roll_out + pitch_out;							// CW
 
 	//back
-	motor_out[MOT_6]     = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out);		// CCW TOP
-	motor_out[MOT_4] 	=  g.rc_3.radio_out - g.rc_2.pwm_out;								// CW
+	motor_out[MOT_6]		 = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out);		// CCW TOP
+	motor_out[MOT_4] 	=	g.rc_3.radio_out - g.rc_2.pwm_out;								// CW
 
 	// Yaw
 	//top
@@ -85,7 +85,7 @@ static void output_motors_armed()
 	//bottom
 	motor_out[MOT_3]		-= g.rc_4.pwm_out;	// CW
 	motor_out[MOT_1]		-= g.rc_4.pwm_out;	// CW
-	motor_out[MOT_4]		-= g.rc_4.pwm_out;  // CW
+	motor_out[MOT_4]		-= g.rc_4.pwm_out;	// CW
 	*/
 
 	// TODO: add stability patch
@@ -93,16 +93,16 @@ static void output_motors_armed()
 	motor_out[MOT_2]	= min(motor_out[MOT_2], 	out_max);
 	motor_out[MOT_3]	= min(motor_out[MOT_3], 	out_max);
 	motor_out[MOT_4] = min(motor_out[MOT_4], 	out_max);
-	motor_out[MOT_5] = min(motor_out[MOT_5],  out_max);
-	motor_out[MOT_6]	= min(motor_out[MOT_6],  out_max);
+	motor_out[MOT_5] = min(motor_out[MOT_5],	out_max);
+	motor_out[MOT_6]	= min(motor_out[MOT_6],	out_max);
 
 	// limit output so motors don't stop
-	motor_out[MOT_1] = max(motor_out[MOT_1],  out_min);
-	motor_out[MOT_2] = max(motor_out[MOT_2],  out_min);
-	motor_out[MOT_3] = max(motor_out[MOT_3],  out_min);
-	motor_out[MOT_4] = max(motor_out[MOT_4],  out_min);
-	motor_out[MOT_5] = max(motor_out[MOT_5],  out_min);
-	motor_out[MOT_6] = max(motor_out[MOT_6],  out_min);
+	motor_out[MOT_1] = max(motor_out[MOT_1],	out_min);
+	motor_out[MOT_2] = max(motor_out[MOT_2],	out_min);
+	motor_out[MOT_3] = max(motor_out[MOT_3],	out_min);
+	motor_out[MOT_4] = max(motor_out[MOT_4],	out_min);
+	motor_out[MOT_5] = max(motor_out[MOT_5],	out_min);
+	motor_out[MOT_6] = max(motor_out[MOT_6],	out_min);
 
 	#if CUT_MOTORS == ENABLED
 	// if we are not sending a throttle output, we cut the motors
@@ -118,8 +118,8 @@ static void output_motors_armed()
 
 	// this filter slows the acceleration of motors vs the deceleration
 	// Idea by Denny Rowland to help with his Yaw issue
-	for(int8_t m = 0; m <= 6; m++ ) {
-    	int i = ch_of_mot(m);
+	for(int8_t m = 0; m <= 6; m++){
+			int i = ch_of_mot(m);
 		if(motor_filtered[i] < motor_out[i]){
 			motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
 		}else{
@@ -152,7 +152,7 @@ static void output_motors_disarmed()
 	}
 
 	// fill the motor_out[] array for HIL use
-	for (unsigned char i = 0; i < 8; i++) {
+	for (unsigned char i = 0; i < 8; i++){
 		motor_out[i] = g.rc_3.radio_min;
 	}
 
@@ -174,21 +174,35 @@ static void output_motor_test()
 	motor_out[MOT_5] = g.rc_3.radio_min;
 	motor_out[MOT_6] = g.rc_3.radio_min;
 
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
+	delay(5000);
+	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
+	delay(300);
 
-	if(g.rc_1.control_in > 3000){	// right
-		motor_out[MOT_1] += 100;
-		motor_out[MOT_5] += 100;
-	}
+	APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
+	delay(300);
 
-	if(g.rc_1.control_in < -3000){	// left
-		motor_out[MOT_2] += 100;
-		motor_out[MOT_3] += 100;
-	}
+	APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
+	delay(300);
 
-	if(g.rc_2.control_in > 3000){	// back
-		motor_out[MOT_6] += 100;
-		motor_out[MOT_4] += 100;
-	}
+	APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
+	delay(300);
+
+	APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
+	delay(3000);
+	APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
+	delay(300);
 
 	APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
 	APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);