diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 1c034d99a1..d7d00c2068 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -22,6 +22,7 @@
 	HEXA_FRAME
 	Y6_FRAME
 	OCTA_FRAME
+	HELI_FRAME
 	*/
 
 #define FRAME_ORIENTATION X_FRAME
@@ -72,4 +73,4 @@
 #define FR_LED AN12  // Mega PE4 pin, OUT7
 #define RE_LED AN14  // Mega PE5 pin, OUT6
 #define RI_LED AN10  // Mega PH4 pin, OUT5
-#define LE_LED AN8  // Mega PH5 pin, OUT4
+#define LE_LED AN8  // Mega PH5 pin, OUT4
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 82cc7ce863..4c07abf053 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -29,6 +29,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <APM_RC.h>         // ArduPilot Mega RC Library
 #include <AP_GPS.h>         // ArduPilot GPS library
 #include <Wire.h>			// Arduino I2C lib
+#include <SPI.h>
 #include <DataFlash.h>      // ArduPilot Mega Flash Memory Library
 #include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
 #include <APM_BMP085.h>     // ArduPilot Mega BMP085 Library
@@ -228,6 +229,12 @@ const char* flight_mode_strings[] = {
 int motor_out[8];
 Vector3f omega;
 
+// Heli
+// ----
+float heli_rollFactor[3], heli_pitchFactor[3];  // only required for 3 swashplate servos
+int heli_servo_min[3], heli_servo_max[3];       // same here.  for yaw servo we use heli_servo4_min/max parameter directly
+int heli_servo_out[4];
+
 // Failsafe
 // --------
 boolean 	failsafe;						// did our throttle dip below the failsafe value?
@@ -1330,6 +1337,22 @@ void tuning(){
 
 	#elif CHANNEL_6_TUNING == CH6_PMAX
 		g.pitch_max.set(g.rc_6.control_in * 2);  // 0 to 2000
+		
+	#elif CHANNEL_6_TUNING == CH6_YAW_KP
+	    // yaw heading
+		g.pid_yaw.kP((float)g.rc_6.control_in / 200.0);  // range from 0.0 ~ 5.0
+		
+	#elif CHANNEL_6_TUNING == CH6_YAW_KD
+	    // yaw heading
+		g.pid_yaw.kD((float)g.rc_6.control_in / 1000.0);
+	
+	#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
+	    // yaw rate
+		g.pid_acro_rate_yaw.kP((float)g.rc_6.control_in / 1000.0);
+		
+	#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
+	    // yaw rate
+		g.pid_acro_rate_yaw.kD((float)g.rc_6.control_in / 1000.0);
 
 	#endif
 }
@@ -1365,4 +1388,4 @@ void update_nav_yaw()
 	}else if(yaw_tracking == MAV_ROI_WPNEXT){
 		nav_yaw = target_bearing;
 	}
-}
+}
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index a6399771fe..14222ae74f 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -97,6 +97,24 @@ public:
         k_param_flight_modes,
         k_param_esc_calibrate,
 
+		//
+		// 200: Heli
+		//
+		k_param_heli_servo_1 = 200,
+		k_param_heli_servo_2,
+		k_param_heli_servo_3,
+		k_param_heli_servo_4,
+		k_param_heli_servo1_pos ,
+		k_param_heli_servo2_pos,
+		k_param_heli_servo3_pos,
+		k_param_heli_roll_max,
+		k_param_heli_pitch_max,
+		k_param_heli_collective_min,
+		k_param_heli_collective_max,
+		k_param_heli_collective_mid,
+		k_param_heli_ext_gyro_enabled,
+		k_param_heli_ext_gyro_gain,  // 213
+
         //
         // 220: Waypoint data
         //
@@ -183,6 +201,14 @@ public:
 	AP_Int8		esc_calibrate;
 	AP_Int8		frame_orientation;
 
+	// Heli
+	RC_Channel	heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4;  // servos for swash plate and tail
+	AP_Int16	heli_servo1_pos, heli_servo2_pos, heli_servo3_pos;       // servo positions (3 because we don't need pos for tail servo)
+	AP_Int16	heli_roll_max, heli_pitch_max;   // maximum allowed roll and pitch of swashplate
+	AP_Int16	heli_coll_min, heli_coll_max, heli_coll_mid;    // min and max collective.  mid = main blades at zero pitch
+	AP_Int8		heli_ext_gyro_enabled;   // 0 = no external tail gyro, 1 = external tail gyro
+	AP_Int16	heli_ext_gyro_gain;      // radio output 1000~2000 (value output on CH_7)
+	
     // RC channels
 	RC_Channel	rc_1;
 	RC_Channel	rc_2;
@@ -249,6 +275,21 @@ public:
 
         frame_orientation 		(FRAME_ORIENTATION, 		k_param_frame_orientation, 				PSTR("FRAME")),
 
+		heli_servo_1			(k_param_heli_servo_1,		PSTR("HS1_")),
+		heli_servo_2			(k_param_heli_servo_2,		PSTR("HS2_")),
+		heli_servo_3			(k_param_heli_servo_3,		PSTR("HS3_")),
+		heli_servo_4			(k_param_heli_servo_4,		PSTR("HS4_")),
+		heli_servo1_pos			(-60,						k_param_heli_servo1_pos,				PSTR("SV1_POS_")),
+		heli_servo2_pos			(60,						k_param_heli_servo2_pos,				PSTR("SV2_POS_")),
+		heli_servo3_pos			(180,						k_param_heli_servo3_pos,				PSTR("SV3_POS_")),		
+		heli_roll_max			(4500,						k_param_heli_roll_max,					PSTR("ROL_MAX_")),
+		heli_pitch_max			(4500,						k_param_heli_pitch_max,					PSTR("PIT_MAX_")),
+		heli_coll_min			(1000,						k_param_heli_collective_min,			PSTR("COL_MIN_")),
+		heli_coll_max			(2000,						k_param_heli_collective_max,			PSTR("COL_MAX_")),
+		heli_coll_mid			(1500,						k_param_heli_collective_mid,			PSTR("COL_MID_")),
+		heli_ext_gyro_enabled	(0,							k_param_heli_ext_gyro_enabled,			PSTR("GYR_ENABLE_")),
+		heli_ext_gyro_gain		(1000,						k_param_heli_ext_gyro_gain,				PSTR("GYR_GAIN_")),
+		
         // RC channel           group key                   name
         //----------------------------------------------------------------------
         rc_1					(k_param_rc_1,		PSTR("RC1_")),
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 87e6c38821..f7aa40c9f7 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -20,6 +20,7 @@
 #define HEXA_FRAME 2
 #define Y6_FRAME 3
 #define OCTA_FRAME 4
+#define HELI_FRAME 5
 
 #define PLUS_FRAME 0
 #define X_FRAME 1
@@ -120,6 +121,10 @@
 #define CH6_SONAR_KD 6
 #define CH6_Y6_SCALING 7
 #define CH6_PMAX 8
+#define	CH6_YAW_KP 9
+#define	CH6_YAW_KD 10
+#define	CH6_YAW_RATE_KP 11
+#define	CH6_YAW_RATE_KD 12
 
 // nav byte mask
 // -------------
@@ -314,10 +319,6 @@
 #define PIEZO_PIN AN5           //Last pin on the back ADC connector
 
 
-
-// sonar
-#define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
-
 // Hardware Parameters
 #define SLIDE_SWITCH_PIN 40
 #define PUSHBUTTON_PIN 41
diff --git a/ArduCopterMega/heli.pde b/ArduCopterMega/heli.pde
new file mode 100644
index 0000000000..0902c45c0c
--- /dev/null
+++ b/ArduCopterMega/heli.pde
@@ -0,0 +1,170 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if FRAME_CONFIG ==	HELI_FRAME
+
+int heli_manual_override = false;
+float rollPitch_impact_on_collective = 0;
+
+void heli_init_swash()
+{
+    int i;
+	int tilt_max[CH_3+1];
+	int total_tilt_max = 0;
+	
+	// swash servo initialisation
+	g.heli_servo_1.set_range(0,1000);
+	g.heli_servo_2.set_range(0,1000);
+	g.heli_servo_3.set_range(0,1000);
+	g.heli_servo_4.set_angle(4500);
+	//g.heli_servo_4.radio_min = 1000;  // required?
+	//g.heli_servo_4.radio_max = 2000;
+	
+	// pitch factors
+	heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos));
+	heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos));
+	heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos));
+	
+	// roll factors
+    heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90));
+	heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90));
+	heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90));
+	
+	// collective min / max
+	total_tilt_max = 0;
+	for( i=CH_1; i<=CH_3; i++ ) {
+	    tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100;
+		total_tilt_max = max(total_tilt_max,tilt_max[i]);
+	}
+	
+	//if( reset_collective == false ) {
+	//	g.heli_coll_min = total_tilt_max;
+	//	g.heli_coll_max = 1000 - total_tilt_max;
+	//}
+	
+	// servo min/max values - or should I use set_range?
+	g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1];
+	g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1];
+	g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2];
+	g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2];
+	g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3];
+	g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
+}
+
+void heli_move_servos_to_mid()
+{
+	heli_move_swash(0,0,1500,0);
+}
+
+//
+// heli_move_swash - moves swash plate to attitude of parameters passed in
+//                 - expected ranges:
+//                       roll : -4500 ~ 4500   // should be -500 to 500?
+//                       pitch: -4500 ~ 4500
+//                       collective: 1000 ~ 2000
+//                       yaw:   -4500 ~ 4500??
+//
+void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
+{
+    // ensure values are acceptable:
+	roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
+	pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
+	coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max);
+
+	// swashplate servos
+	g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500);
+	if( g.heli_servo_1.get_reverse() )
+	    g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out;
+		
+	g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500);
+	if( g.heli_servo_2.get_reverse() )
+		g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out;
+		
+	g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500);
+	if( g.heli_servo_3.get_reverse() )
+	    g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out;
+		
+	if( g.heli_servo_4.get_reverse() )
+		g.heli_servo_4.servo_out = -yaw_out;  // should probably just use rc_4 directly like we do for a tricopter
+	else
+		g.heli_servo_4.servo_out = yaw_out;
+	
+	// use servo_out to calculate pwm_out and radio_out
+	g.heli_servo_1.calc_pwm();
+	g.heli_servo_2.calc_pwm();
+	g.heli_servo_3.calc_pwm();
+	g.heli_servo_4.calc_pwm();	
+	
+	// actually move the servos
+	APM_RC.OutputCh(CH_1, g.heli_servo_1.servo_out);
+	APM_RC.OutputCh(CH_2, g.heli_servo_2.servo_out);
+	APM_RC.OutputCh(CH_3, g.heli_servo_3.servo_out);
+	//APM_RC.OutputCh(CH_4, g.heli_servo_4.servo_out);
+	APM_RC.OutputCh(CH_4, g.heli_servo_4.radio_out);
+	
+	// output gyro value
+	if( g.heli_ext_gyro_enabled ) {
+	    APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain);
+	}
+
+	// InstantPWM
+	APM_RC.Force_Out0_Out1();
+	APM_RC.Force_Out2_Out3();
+	
+	// debug
+	//Serial.printf_P(PSTR("4: r%d \tcp:%d \tcol:%d \ty:%d \tout:%d \tpwm:%d \trOut:%d \ttrim:%d\n"), roll_out, pitch_out, coll_out, yaw_out, (int)g.heli_servo_4.servo_out, (int)g.heli_servo_4.pwm_out, (int)g.heli_servo_4.radio_out, (int)g.heli_servo_4.radio_trim);
+	//Serial.printf_P(PSTR("4: y:%d \tout:%d \tpwm:%d \trOut:%d \ttrim:%d\n"), yaw_out, (int)g.heli_servo_4.servo_out, (int)g.heli_servo_4.pwm_out, (int)g.heli_servo_4.radio_out, (int)g.heli_servo_4.radio_trim);
+	//Serial.printf_P(PSTR("4: y:%d \tro:%d\n"), yaw_out, (int)g.heli_servo_4.radio_out);
+}
+
+// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
+void output_motors_armed()
+{
+    //static int counter = 0;
+	g.rc_1.calc_pwm();
+	g.rc_2.calc_pwm();
+	g.rc_3.calc_pwm();
+	g.rc_4.calc_pwm();
+
+	if( heli_manual_override ) {
+	    // straight pass through from radio inputs to swash plate
+	    heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in );
+		
+	    /*Serial.printf_P(  PSTR("1: %d/%d \t2:%d/%d \t3:%d/%d \t4:%d/%d\n"), 
+			(int)g.rc_1.control_in, (int)g.rc_1.servo_out, 
+			(int)g.rc_2.control_in, (int)g.rc_2.servo_out, 
+			(int)g.rc_3.radio_in,   (int)g.rc_3.servo_out, 
+			(int)g.rc_4.control_in, (int)g.rc_4.servo_out );*/
+	}else{
+	    // collective pitch compensation for yaw/roll.  This probably belongs somewhere else
+		//Matrix3f temp 	= dcm.get_dcm_matrix();
+		//rollPitch_impact_on_collective = 1.0 * (g.rc_3.radio_in-g.heli_coll_mid) * (1.0 - temp.c.z);
+		//rollPitch_impact_on_collective = constrain(rollPitch_impact_on_collective,0,100);
+		
+		/*counter++;
+		if( counter > 20 ) {
+		    counter = 0;
+			Serial.printf_P( PSTR("dcm:%f4.1\t rc3:%d\t cm:%d\t imp:%d\n"), 
+				temp.c.z, 
+				(int)g.rc_3.radio_in,
+				(int)g.heli_coll_mid,
+				(int)rollPitch_impact_on_collective );
+		}*/
+	
+	    // source inputs from attitude controller (except for collective pitch)
+	    //heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_in + rollPitch_impact_on_collective, g.rc_4.servo_out );  // to allow control by PIDs except for collective
+		heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out );  // to allow control by PIDs except for collective
+	}
+}
+
+// for helis - armed or disarmed we allow servos to move
+void output_motors_disarmed()
+{
+	//heli_move_servos_to_mid();
+	output_motors_armed();
+}
+
+void output_motor_test()
+{
+}
+
+#endif // HELI_FRAME
\ No newline at end of file
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index 00ea3dddb7..942302c3aa 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -86,10 +86,14 @@ void init_rc_out()
 
 void output_min()
 {
-	APM_RC.OutputCh(CH_1, 	g.rc_3.radio_min);					// Initialization of servo outputs
-	APM_RC.OutputCh(CH_2, 	g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_3, 	g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_4, 	g.rc_3.radio_min);
+    #if FRAME_CONFIG ==	HELI_FRAME
+        heli_move_servos_to_mid();
+	#else
+	    APM_RC.OutputCh(CH_1, 	g.rc_3.radio_min);					// Initialization of servo outputs
+	    APM_RC.OutputCh(CH_2, 	g.rc_3.radio_min);
+	    APM_RC.OutputCh(CH_3, 	g.rc_3.radio_min);
+	    APM_RC.OutputCh(CH_4, 	g.rc_3.radio_min);
+	#endif
 
 	APM_RC.OutputCh(CH_7,   g.rc_3.radio_min);
     APM_RC.OutputCh(CH_8,   g.rc_3.radio_min);
@@ -111,9 +115,10 @@ void read_radio()
 	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
 	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
 
-
+#if FRAME_CONFIG != HELI_FRAME
 	// limit our input to 800 so we can still pitch and roll
 	g.rc_3.control_in = min(g.rc_3.control_in, 800);
+#endif
 
 	//throttle_failsafe(g.rc_3.radio_in);
 
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index f1d3de75d0..e34ed3a3f7 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -15,6 +15,10 @@ static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_esc				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_show				(uint8_t argc, const Menu::arg *argv);
+#if FRAME_CONFIG == HELI_FRAME
+static int8_t	setup_heli				(uint8_t argc, const Menu::arg *argv);
+static int8_t	setup_gyro				(uint8_t argc, const Menu::arg *argv);
+#endif
 
 // Command/function table for the setup menu
 const struct Menu::command setup_menu_commands[] PROGMEM = {
@@ -33,6 +37,10 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"compass",			setup_compass},
 	{"offsets",			setup_mag_offset},
 	{"declination",		setup_declination},
+#if FRAME_CONFIG == HELI_FRAME	
+	{"heli",			setup_heli},
+	{"gyro",			setup_gyro},
+#endif
 	{"show",			setup_show}
 };
 
@@ -81,6 +89,10 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_flight_modes();
 	report_imu();
 	report_compass();
+#if FRAME_CONFIG == HELI_FRAME
+	report_heli();
+	report_gyro();
+#endif
 
 	AP_Var_menu_show(argc, argv);
 	return(0);
@@ -408,6 +420,232 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
 	return 0;
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+
+// Perform heli setup.
+// Called by the setup menu 'radio' command.
+static int8_t
+setup_heli(uint8_t argc, const Menu::arg *argv)
+{
+
+	uint8_t active_servo = 0;
+	int value = 0;
+	int temp;
+	int state = 0;   // 0 = set rev+pos, 1 = capture min/max
+	int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail;
+
+	// initialise swash plate
+	heli_init_swash();
+	
+	// source swash plate movements directly from 
+	heli_manual_override = true;
+	
+	// display initial settings
+	report_heli();
+	
+	// display help
+	Serial.printf_P(PSTR("Instructions:"));
+	print_divider();
+	Serial.printf_P(PSTR("\td\t\tdisplay settings\n"));
+	Serial.printf_P(PSTR("\t1~4\t\tselect servo\n"));
+	Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
+	Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
+	Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
+	Serial.printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
+	Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
+	Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
+	Serial.printf_P(PSTR("\tx\t\texit & save\n"));
+	
+	// start capturing
+	while( value != 'x' ) {
+	
+	    // read radio although we don't use it yet
+	    read_radio();
+		
+		// record min/max 
+		if( state == 1 ) {
+		    if( abs(g.rc_1.control_in) > max_roll )
+			    max_roll = abs(g.rc_1.control_in);
+			if( abs(g.rc_2.control_in) > max_pitch )
+			    max_pitch = abs(g.rc_2.control_in);
+			if( g.rc_3.radio_in < min_coll )
+			    min_coll = g.rc_3.radio_in;
+			if( g.rc_3.radio_in > max_coll )
+			    max_coll = g.rc_3.radio_in;
+			min_tail = min(g.rc_4.radio_in, min_tail);
+			max_tail = max(g.rc_4.radio_in, max_tail);
+			//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
+		}
+	
+	    if( Serial.available() ) {
+		    value = Serial.read();
+			
+			// process the user's input
+			switch( value ) {
+				case '1':
+					active_servo = CH_1;
+					break;
+				case '2':
+					active_servo = CH_2;
+					break;
+				case '3':
+					active_servo = CH_3;
+					break;
+				case '4':
+					active_servo = CH_4;
+					break;
+				case 'a':					
+				case 'A':
+					heli_get_servo(active_servo)->radio_trim += 10;
+					break;
+				case 'c':
+				case 'C':
+				    if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) {
+						g.heli_coll_mid = g.rc_3.radio_in;
+						Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid);
+					}
+					break;
+				case 'd':
+				case 'D':
+					// display settings
+					report_heli();
+					break;
+				case 'm':
+				case 'M':
+				    if( state == 0 ) {
+					    state = 1;  // switch to capture min/max mode
+						Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"),active_servo+1, temp);
+						
+						// reset servo ranges
+						g.heli_roll_max = g.heli_pitch_max = 4500;
+						g.heli_coll_min = 1000;
+						g.heli_coll_max = 2000;
+						g.heli_servo_4.radio_min = 1000;
+						g.heli_servo_4.radio_max = 2000;
+						
+						// set sensible values in temp variables
+						max_roll = abs(g.rc_1.control_in);
+						max_pitch = abs(g.rc_2.control_in);
+						min_coll = 2000;
+						max_coll = 1000;
+						min_tail = max_tail = abs(g.rc_4.radio_in);
+					}else{
+					    state = 0;  // switch back to normal mode
+						// double check values aren't totally terrible
+						if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
+						    Serial.printf_P(PSTR("Invalid min/max captured roll:%d,  pitch:%d,  collective min: %d max: %d,  tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail);
+						else{
+						    g.heli_roll_max = max_roll;
+							g.heli_pitch_max = max_pitch;
+							g.heli_coll_min = min_coll;
+							g.heli_coll_max = max_coll;
+							g.heli_servo_4.radio_min = min_tail;
+							g.heli_servo_4.radio_max = max_tail;
+							
+							// reinitialise swash
+							heli_init_swash();
+							
+							// display settings
+							report_heli();
+						}
+					}
+					break;
+				case 'p':
+				case 'P':
+					temp = read_num_from_serial();
+					if( temp >= -360 && temp <= 360 ) {
+						if( active_servo == CH_1 )
+							g.heli_servo1_pos = temp;
+						if( active_servo == CH_2 )
+							g.heli_servo2_pos = temp;
+						if( active_servo == CH_3 )
+							g.heli_servo3_pos = temp;
+						heli_init_swash();
+						Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp); 
+					}
+					break;
+				case 'r':
+				case 'R':
+				    heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse());
+					break;
+				case 't':
+				case 'T':
+					temp = read_num_from_serial();
+					if( temp > 1000 )
+					    temp -= 1500;
+					if( temp > -500 && temp < 500 ) {
+					    heli_get_servo(active_servo)->radio_trim = 1500 + temp;
+						heli_init_swash();
+						Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); 
+					}
+					break;
+				case 'z':			
+				case 'Z':
+					heli_get_servo(active_servo)->radio_trim -= 10;
+					break;
+			}
+		}
+		
+		// allow swash plate to move
+		output_motors_armed();
+		
+		delay(20);
+	}
+
+	// display final settings
+	report_heli();
+	
+	// save to eeprom
+	g.heli_servo_1.save_eeprom();
+	g.heli_servo_2.save_eeprom();
+	g.heli_servo_3.save_eeprom();
+	g.heli_servo_4.save_eeprom();
+	g.heli_servo1_pos.save();
+	g.heli_servo2_pos.save();
+	g.heli_servo3_pos.save();
+	g.heli_roll_max.save();
+	g.heli_pitch_max.save();
+	g.heli_coll_min.save();
+	g.heli_coll_max.save();
+	g.heli_coll_mid.save();
+	
+	// return swash plate movements to attitude controller
+	heli_manual_override = false;
+
+	return(0);
+}
+
+// setup for external tail gyro (for heli only)
+static int8_t
+setup_gyro(uint8_t argc, const Menu::arg *argv)
+{
+	if (!strcmp_P(argv[1].str, PSTR("on"))) {
+		g.heli_ext_gyro_enabled.set_and_save(true);
+
+		// optionally capture the gain
+		if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) {
+		    g.heli_ext_gyro_gain = argv[2].i;
+			g.heli_ext_gyro_gain.save();
+		}
+		
+	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
+		g.heli_ext_gyro_enabled.set_and_save(false);
+
+    // capture gain if user simply provides a number
+	} else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) {
+	    g.heli_ext_gyro_enabled.set_and_save(true);
+		g.heli_ext_gyro_gain = argv[1].i;
+		g.heli_ext_gyro_gain.save();
+
+	}else{
+		Serial.printf_P(PSTR("\nOptions:[on, off] gain\n"));
+	}
+
+	report_gyro();
+	return 0;
+}
+
+#endif // FRAME_CONFIG == HELI
 void clear_offsets()
 {
 	Vector3f _offsets;
@@ -583,12 +821,16 @@ void report_frame()
 	Serial.printf_P(PSTR("Y6 frame\n"));
 #elif FRAME_CONFIG == OCTA_FRAME
 	Serial.printf_P(PSTR("Octa frame\n"));
+#elif FRAME_CONFIG == HELI_FRAME
+	Serial.printf_P(PSTR("Heli frame\n"));
 #endif
 
+#if FRAME_CONFIG != HELI_FRAME
 	if(g.frame_orientation == X_FRAME)
 		Serial.printf_P(PSTR("X mode\n"));
 	else if(g.frame_orientation == PLUS_FRAME)
 		Serial.printf_P(PSTR("+ mode\n"));
+#endif
 
 	print_blanks(2);
 }
@@ -712,6 +954,41 @@ void report_flight_modes()
 	print_blanks(2);
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+void report_heli()
+{
+	Serial.printf_P(PSTR("Heli\n"));
+	print_divider();
+	
+	// main servo settings
+	Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
+	Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo1_pos, (int)g.heli_servo_1.radio_min, (int)g.heli_servo_1.radio_max, (int)g.heli_servo_1.get_reverse());
+	Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo2_pos, (int)g.heli_servo_2.radio_min, (int)g.heli_servo_2.radio_max, (int)g.heli_servo_2.get_reverse());
+	Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo3_pos, (int)g.heli_servo_3.radio_min, (int)g.heli_servo_3.radio_max, (int)g.heli_servo_3.get_reverse());
+	Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)g.heli_servo_4.radio_min, (int)g.heli_servo_4.radio_max, (int)g.heli_servo_4.get_reverse());
+		
+	Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
+	Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
+	Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
+		
+	print_blanks(2);
+}
+
+void report_gyro()
+{
+
+	Serial.printf_P(PSTR("External Gyro:\n"));
+	print_divider();
+
+	print_enabled( g.heli_ext_gyro_enabled );
+	if( g.heli_ext_gyro_enabled )
+	    Serial.printf_P(PSTR("gain: %d"),(int)g.heli_ext_gyro_gain);
+		
+	print_blanks(2);
+}
+
+#endif // FRAME_CONFIG == HELI_FRAME
+
 /***************************************************************************/
 // CLI utilities
 /***************************************************************************/
@@ -834,3 +1111,35 @@ print_gyro_offsets(void)
 						(float)imu.gz());
 }
 
+RC_Channel *
+heli_get_servo(int servo_num){
+	if( servo_num == CH_1 )
+	    return &g.heli_servo_1;
+	if( servo_num == CH_2 )
+	    return &g.heli_servo_2;
+	if( servo_num == CH_3 )
+	    return &g.heli_servo_3;
+	if( servo_num == CH_4 )
+	    return &g.heli_servo_4;
+	return NULL;
+}
+
+// Used to read integer values from the serial port
+int read_num_from_serial() {
+	byte index = 0;
+	byte timeout = 0;
+	char data[5] = "";
+
+	do {
+		if (Serial.available() == 0) {
+			delay(10);
+			timeout++;
+		}else{
+			data[index] = Serial.read();
+			timeout = 0;
+			index++;
+		}
+	}while (timeout < 5 && index < 5);
+	
+	return atoi(data);
+}
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index bfded12549..2b31eb6ff0 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -134,6 +134,10 @@ void init_ardupilot()
 		APM_RC.setHIL(rc_override);
 	}
 	#endif
+	
+    #if FRAME_CONFIG ==	HELI_FRAME 
+    heli_init_swash();  // heli initialisation
+	#endif	
 
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 75c804c654..ecfb060e5a 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -699,6 +699,22 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
 
 		#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
 			Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
+			
+		#elif CHANNEL_6_TUNING == CH6_YAW_KP
+			// yaw heading
+			Serial.printf_P(PSTR("yaw kP: %1.3f\n"), ((float)g.rc_6.control_in / 200.0));  // range from 0 ~ 5.0
+			
+		#elif CHANNEL_6_TUNING == CH6_YAW_KD
+			// yaw heading
+			Serial.printf_P(PSTR("yaw kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
+		
+		#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
+			// yaw rate
+			Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
+			
+		#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
+			// yaw rate
+			Serial.printf_P(PSTR("yaw rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
 		#endif
 		if(Serial.available() > 0){
 			return (0);