diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 7b871153ef..4881a787bf 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -263,7 +263,7 @@ float cos_pitch_x 	= 1;
 float cos_yaw_x 	= 1;
 float sin_pitch_y, sin_yaw_y, sin_roll_y;
 float sin_nav_y, cos_nav_x;					// used in calc_waypoint_nav
-long initial_simple_bearing;				// used for Simple mode
+long initial_simple_bearing = -1;				// used for Simple mode
 
 // Airspeed
 // --------
@@ -426,9 +426,7 @@ unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
 
 byte			counter_one_herz;
-
-byte			GPS_failure_counter = 3;
-bool			GPS_disabled 		= false;
+bool			GPS_disabled 		= true;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
@@ -522,12 +520,7 @@ void medium_loop()
 		case 0:
 			medium_loopCounter++;
 
-			if(GPS_failure_counter > 0){
-				update_GPS();
-
-			}else if(GPS_failure_counter == 0){
-				GPS_disabled = true;
-			}
+			update_GPS();
 			//readCommands();
 
 			if(g.compass_enabled){
@@ -780,13 +773,15 @@ void super_slow_loop()
 
 void update_GPS(void)
 {
+	if(Serial1.available() == 0){
+		return;
+	}
+
 	g_gps->update();
 	update_GPS_light();
 
-	if(g_gps->new_data)
-		GPS_failure_counter = 3;
-
     if (g_gps->new_data && g_gps->fix) {
+		GPS_disabled = false;
 
 		// XXX We should be sending GPS data off one of the regular loops so that we send
 		// no-GPS-fix data too
@@ -828,10 +823,6 @@ void update_GPS(void)
 
 		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
 		current_loc.lat = g_gps->latitude;		// Lat * 10 * *7
-
-	}else{
-		if(GPS_failure_counter > 0)
-			GPS_failure_counter--;
 	}
 }
 
diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde
index 4702018672..24c293a77f 100644
--- a/ArduCopterMega/control_modes.pde
+++ b/ArduCopterMega/control_modes.pde
@@ -43,7 +43,7 @@ void reset_control_switch()
 {
 	oldSwitchPosition = -1;
 	read_control_switch();
-	SendDebug("MSG: reset_control_switch");
+	SendDebug("MSG: reset_control_switch ");
 	SendDebugln(oldSwitchPosition , DEC);
 }
 
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index ad5e3779a2..fd55719b5e 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -221,7 +221,7 @@ void startup_ground(void)
 		delay(GROUND_START_DELAY * 1000);
 	#endif
 
-	setup_show(NULL,NULL);
+	//setup_show(NULL,NULL);
 
 	// Output waypoints for confirmation
 	// --------------------------------
@@ -254,6 +254,9 @@ void startup_ground(void)
 	// -------------------
 	init_commands();
 
+    // clear out the GPS buffer
+	Serial1.flush();
+
 	gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
 }
 
@@ -293,7 +296,14 @@ void set_mode(byte mode)
 			break;
 
 		case SIMPLE:
-			initial_simple_bearing = dcm.yaw_sensor;
+		    if(g.rc_3.control_in == 0){                         // we are on the ground
+		        if(initial_simple_bearing == -1){               // only set once
+    		    	initial_simple_bearing = dcm.yaw_sensor;
+                }
+		    }else if(initial_simple_bearing == -1){             // we are flying
+		        // don't enter simple mode
+                control_mode = STABILIZE;                       // just stabilize
+		    }
 			break;
 
 		case LOITER:
@@ -310,7 +320,6 @@ void set_mode(byte mode)
 
 	// output control mode to the ground station
 	gcs.send_message(MSG_MODE_CHANGE);
-
 }
 
 void set_failsafe(boolean mode)