From 14d19ac33d398a83ff9ef747e7d2dbf4fc450fb0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 17 Sep 2012 14:43:56 +1000
Subject: [PATCH] ACM: updates for new GPS detection

a GPS can now be attached after startup
---
 ArduCopter/ArduCopter.pde | 10 +---------
 ArduCopter/setup.pde      |  8 --------
 ArduCopter/system.pde     | 31 +------------------------------
 3 files changed, 2 insertions(+), 47 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 800b68d904..20c5523f39 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -905,9 +905,6 @@ static uint8_t auto_disarming_counter;
 // prevents duplicate GPS messages from entering system
 static uint32_t last_gps_time;
 
-// 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;
 // Set true if we have new PWM data to act on from the Radio
 static bool new_radio_frame;
 // Used to auto exit the in-flight leveler
@@ -1485,15 +1482,10 @@ static void update_GPS(void)
     // A counter that is used to grab at least 10 reads before commiting the Home location
     static byte ground_start_count  = 10;
 
-    // return immediately if GPS is not enabled
-    if( !GPS_enabled ) {
-        return;
-    }
-
     g_gps->update();
     update_GPS_light();
 
-    if(gps_watchdog < 30) {
+    if (gps_watchdog < 30) {
         gps_watchdog++;
     }else{
         // after 12 reads we guess we may have lost GPS signal, stop navigating
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 6377a3d856..eeacccc83b 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -1233,14 +1233,6 @@ static void print_wp(struct Location *cmd, byte index)
 	*/
 }
 
-static void report_gps()
-{
-    Serial.printf_P(PSTR("\nGPS\n"));
-    print_divider();
-    print_enabled(GPS_enabled);
-    print_blanks(2);
-}
-
 static void report_version()
 {
     Serial.printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 0e6867033d..ea3bc32065 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -221,7 +221,6 @@ static void init_ardupilot()
     g_gps = &g_gps_driver;
     // GPS Initialization
     g_gps->init(GPS::GPS_ENGINE_AIRBORNE_1G);
-    g_gps->callback = mavlink_delay;
 
     if(g.compass_enabled)
         init_compass();
@@ -253,34 +252,6 @@ static void init_ardupilot()
     Serial.printf_P(PSTR("\nPress ENTER 3 times for CLI\n\n"));
 #endif // CLI_ENABLED
 
-    GPS_enabled = false;
-
-#if HIL_MODE == HIL_MODE_DISABLED
-    // Read in the GPS
-    for (byte counter = 0;; counter++) {
-        g_gps->update();
-        if (g_gps->status() != 0) {
-            GPS_enabled = true;
-            break;
-        }
-
-        if (counter >= 2) {
-            GPS_enabled = false;
-            break;
-        }
-    }
-#else
-    GPS_enabled = true;
-#endif
-
-    // lengthen the idle timeout for gps Auto_detect
-    // ---------------------------------------------
-    g_gps->idleTimeout = 20000;
-
-    // print the GPS status
-    // --------------------
-    report_gps();
-
 #if HIL_MODE != HIL_MODE_ATTITUDE
     // read Baro pressure at ground
     //-----------------------------
@@ -433,7 +404,7 @@ static void set_mode(byte mode)
     }
 
     // nothing but OF_LOITER for OptFlow only
-    if (g.optflow_enabled && GPS_enabled == false) {
+    if (g.optflow_enabled && g_gps->status() != GPS::GPS_OK) {
         if (mode > ALT_HOLD && mode != OF_LOITER)
             mode = STABILIZE;
     }