diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 3c55d491f3..4356ff7596 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -444,9 +444,6 @@ static byte 	ground_start_count	= 5;
 // on the ground or in the air.  Used to decide if a ground start is appropriate if we
 // booted with an air start.
 static int     ground_start_avg;
-// Tracks if GPS is enabled based on statup routine
-// If we do not detect GPS at startup, we stop trying and assume GPS is not connected	
-static bool	GPS_enabled 	= false;	
 static int32_t          gps_base_alt;		
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -885,10 +882,8 @@ static void medium_loop()
 		//-------------------------------
 		case 0:
 			medium_loopCounter++;
-			if(GPS_enabled){
-                update_GPS();
-                calc_gndspeed_undershoot();
-            }
+            update_GPS();
+            calc_gndspeed_undershoot();
             
 //#if LITE == DISABLED
 			#if HIL_MODE != HIL_MODE_ATTITUDE
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index f284bc2d4d..4a1466117a 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -461,7 +461,7 @@ static void do_change_speed()
 
 static void do_set_home()
 {
-	if(next_nonnav_command.p1 == 1 && GPS_enabled) {
+	if(next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK) {
 		init_home();
 	} else {
 		home.id 	= MAV_CMD_NAV_WAYPOINT;
diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde
index bf56fb8b53..79f3915784 100644
--- a/APMrover2/navigation.pde
+++ b/APMrover2/navigation.pde
@@ -71,8 +71,10 @@ void calc_distance_error()
 
 static void calc_gndspeed_undershoot()
 {
-    // Function is overkill, but here in case we want to add filtering later
-    groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - ground_speed) : 0;
+    if (g_gps->status() == GPS::GPS_OK) {
+        // Function is overkill, but here in case we want to add filtering later
+        groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - ground_speed) : 0;
+    }
 }
 
 static void calc_bearing_error()
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index affc141771..c61aa1f4a6 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -235,7 +235,6 @@ static void init_ardupilot()
 	g_gps = &g_gps_driver;
     // GPS initialisation
 	g_gps->init(GPS::GPS_ENGINE_AUTOMOTIVE);
-    g_gps->callback = mavlink_delay;
 
 	//mavlink_system.sysid = MAV_SYSTEM_ID;				// Using g.sysid_this_mav
 	mavlink_system.compid = 1;	//MAV_COMP_ID_IMU;   // We do not check for comp id
@@ -348,21 +347,6 @@ static void startup_ground(void)
 	// -------------------
 	init_commands();
 
-    // Read in the GPS - see if one is connected
-    GPS_enabled = false;
-	for (byte counter = 0; ; counter++) {
-		g_gps->update();
-		if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){
-			GPS_enabled = true;
-			break;
-		}
-
-		if (counter >= 2) {
-			GPS_enabled = false;
-			break;
-	    }
-	}
-
 	// Makes the servos wiggle - 3 times signals ready to fly
 	// -----------------------
 	demo_servos(3);