diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index 29c3a9639a..e734e849e3 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -91,6 +91,7 @@ public:
         k_param_throttle_cruise,
         k_param_flight_mode_channel,
         k_param_flight_modes,
+        k_param_esc_calibrate,
 
         //
         // 220: Waypoint data
@@ -180,7 +181,7 @@ public:
     AP_Int8		current_enabled;
     AP_Int16	milliamp_hours;
     AP_Int8		compass_enabled;
-
+	AP_Int8		esc_calibrate;
 
     // RC channels
 	RC_Channel	rc_1;
@@ -264,6 +265,9 @@ public:
         rc_camera_pitch			(k_param_rc_9,		PSTR("RC9_")),
         rc_camera_roll			(k_param_rc_10,		PSTR("RC10_")),
 
+        esc_calibrate 			(0,         				k_param_esc_calibrate, 				PSTR("ESC")),
+
+
         // PID controller   group key						name				initial P			initial I			initial D			initial imax
         //--------------------------------------------------------------------------------------------------------------------------------------------------------------------
 		pid_acro_rate_roll	(k_param_pid_acro_rate_roll,	PSTR("ACR_RLL_"),	ACRO_RATE_ROLL_P,   ACRO_RATE_ROLL_I,	ACRO_RATE_ROLL_D,	ACRO_RATE_ROLL_IMAX * 100),
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index d129506d71..6145f61106 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -7,8 +7,6 @@ byte failsafeCounter = 0;		// we wait a second to take over the throttle and sen
 
 void init_rc_in()
 {
-	//read_EEPROM_radio();		// read Radio limits
-
 	// set rc channel ranges
 	g.rc_1.set_angle(4500);
 	g.rc_2.set_angle(4500);
@@ -66,6 +64,10 @@ void init_rc_out()
     OCR4B = 0xFFFF;     // PH4, OUT6
     OCR4C = 0xFFFF;     // PH5, OUT5
 
+	// don't fuss if we are calibrating
+	if(g.esc_calibrate == 1)
+		return;
+
     if(g.rc_3.radio_min <= 1200){
         output_min();
     }
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 5d82b3e869..5829e3be55 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -13,6 +13,7 @@ static int8_t	setup_current			(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_sonar				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_compass			(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);
 
 // Command/function table for the setup menu
@@ -24,6 +25,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"pid",				setup_pid},
 	{"radio",			setup_radio},
 	{"motors",			setup_motors},
+	{"esc",				setup_esc},
 	{"level",			setup_accel},
 	{"modes",			setup_flightmodes},
 	{"frame",			setup_frame},
@@ -195,21 +197,53 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 	return(0);
 }
 
+static int8_t
+setup_esc(uint8_t argc, const Menu::arg *argv)
+{
+	Serial.printf_P(PSTR("\nUnplug battery, calibrate as usual.\n Press Enter to cancel.\n"));
+
+	g.esc_calibrate.set_and_save(1);
+
+	while(1){
+		delay(20);
+
+		if(Serial.available() > 0){
+			g.esc_calibrate.set_and_save(0);
+			break;
+		}
+	}
+}
+
+void
+init_esc()
+{
+	Serial.printf_P(PSTR("\nCalibrate ESC\nRestart when done."));
+
+	g.esc_calibrate.set_and_save(0);
+
+	while(1){
+		read_radio();
+		delay(20);
+
+		APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
+		APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
+		APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
+		APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
+
+	}
+}
+
 static int8_t
 setup_motors(uint8_t argc, const Menu::arg *argv)
 {
-	report_frame();
-
-	init_rc_in();
-
 	print_hit_enter();
-	delay(1000);
 
+	report_frame();
+	//init_rc_in();
+	//delay(1000);
 
 	int out_min = g.rc_3.radio_min + 70;
 
-
-
 	while(1){
 		delay(20);
 		read_radio();
@@ -316,6 +350,7 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
 	return(0);
 }
 
+
 static int8_t
 setup_pid(uint8_t argc, const Menu::arg *argv)
 {
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 1343ea0eb3..c07b89627d 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -186,15 +186,19 @@ void init_ardupilot()
 	// menu; they must reset in order to fly.
 	//
 	if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
-		digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
-		Serial.printf_P(PSTR("\n"
-							 "Entering interactive setup mode...\n"
-							 "\n"
-							 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
-							 "Visit the 'setup' menu for first-time configuration.\n\n"));
-		for (;;) {
-			//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
-			main_menu.run();
+		if(g.esc_calibrate == 1){
+			init_esc();
+		}else{
+			digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
+			Serial.printf_P(PSTR("\n"
+								 "Entering interactive setup mode...\n"
+								 "\n"
+								 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
+								 "Visit the 'setup' menu for first-time configuration.\n\n"));
+			for (;;) {
+				//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
+				main_menu.run();
+			}
 		}
 	}
 
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 77ed185225..9dd8e44da3 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -166,10 +166,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 	while(1){
 		delay(20);
 		read_radio();
-		//output_manual_throttle();
-		//g.rc_1.calc_pwm();
-		//g.rc_2.calc_pwm();
-		//g.rc_4.calc_pwm();
+
 
 		Serial.printf_P(PSTR("IN  1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
 							g.rc_1.control_in,