diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index bb4f53876a..826cca5c8b 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -7,7 +7,7 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 1	// 0 = traditional, 1 = rate controlled
-#define YAW_OPTION 1	// 0 = hybrid rate approach, 1 = offset Yaw approach
+#define YAW_OPTION 0	// 0 = hybrid rate approach, 1 = offset Yaw approach
 #define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input.
 
 #define FRAME_CONFIG QUAD_FRAME
@@ -63,7 +63,7 @@
 
 
 
-#define MOTOR_LEDS 0	// 0 = off, 1 = on
+#define MOTOR_LEDS 1	// 0 = off, 1 = on
 
 #define FR_LED AN12  // Mega PE4 pin, OUT7
 #define RE_LED AN14  // Mega PE5 pin, OUT6
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index fd8d7f5745..3ec3f3bf61 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -436,11 +436,9 @@ byte 			slow_loopCounter;
 int 			superslow_loopCounter;
 byte			flight_timer;				// for limiting the execution of flight mode thingys
 
-unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
-unsigned long 	nav2_loopTimer;				// used to track the elapsed ime for GPS nav
 
 unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
-unsigned long 	dTnav2;						// Delta Time in milliseconds for navigation computations
+unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
 unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
 
@@ -577,11 +575,6 @@ void medium_loop()
 		case 1:
 			medium_loopCounter++;
 
-			// calc pitch and roll to target
-			// -----------------------------
-			dTnav2 				= millis() - nav2_loopTimer;
-			nav2_loopTimer 		= millis();
-
 			// hack to stop navigation in Simple mode
 			if (control_mode == SIMPLE)
 				break;
@@ -671,6 +664,7 @@ void medium_loop()
 		//---------------------------------
 		case 4:
 			medium_loopCounter = 0;
+
 			delta_ms_medium_loop	= millis() - medium_loopTimer;
 			medium_loopTimer    	= millis();
 
@@ -1171,7 +1165,8 @@ void update_navigation()
 			update_nav_yaw();
 		case LOITER:
 			// are we Traversing or Loitering?
-			wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
+			//wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
+			wp_control = LOITER_MODE;
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 47a4d6704a..72d3f7886a 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -43,7 +43,7 @@ bool check_missed_wp()
 	return (abs(temp) > 10000);	//we pased the waypoint by 10 °
 }
 
-#define DIST_ERROR_MAX 700
+#define DIST_ERROR_MAX 1800
 void calc_loiter_nav()
 {
 	/*
diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde
index 8200bb1473..00ea3dddb7 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -22,7 +22,7 @@ void init_rc_in()
 	g.rc_1.dead_zone = 60;		// 60 = .6 degrees
 	g.rc_2.dead_zone = 60;
 	g.rc_3.dead_zone = 60;
-	g.rc_4.dead_zone = 200;
+	g.rc_4.dead_zone = 300;
 
 	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index f37d16578f..a02729ea4b 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -271,8 +271,10 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 		g.frame_orientation.set_and_save(X_FRAME);
 	} else if (!strcmp_P(argv[1].str, PSTR("p"))) {
 		g.frame_orientation.set_and_save(PLUS_FRAME);
+	} else if (!strcmp_P(argv[1].str, PSTR("+"))) {
+		g.frame_orientation.set_and_save(PLUS_FRAME);
 	}else{
-		Serial.printf_P(PSTR("\nOptions:[x,p]\n"));
+		Serial.printf_P(PSTR("\nOptions:[x,+]\n"));
 		report_frame();
 		return 0;
 	}
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index d57fa09cef..1a02f6dbb3 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.15 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.16 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -439,7 +439,7 @@ void update_GPS_light(void)
 
 void update_motor_light(void)
 {
-	if(motor_armed == true){
+	if(motor_armed == false){
 		motor_light = !motor_light;
 
 		// blink