From fe6ad3579cc3a2bcea832a6b2848fff120fab1ca Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 22 Nov 2013 11:51:48 +0900
Subject: [PATCH] Copter: log GPS messages even when no fix

---
 ArduCopter/ArduCopter.pde | 49 +++++++++++++++++++++------------------
 1 file changed, 26 insertions(+), 23 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 51bcf5b615..95b1cab61e 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -794,8 +794,6 @@ static uint32_t fast_loopTimer;
 static uint16_t mainLoop_count;
 // Loiter timer - Records how long we have been in loiter
 static uint32_t rtl_loiter_start_time;
-// prevents duplicate GPS messages from entering system
-static uint32_t last_gps_time;
 
 // Used to exit the roll and pitch auto trim function
 static uint8_t auto_trim_counter;
@@ -1282,37 +1280,42 @@ static void update_optical_flow(void)
 // called at 50hz
 static void update_GPS(void)
 {
-    // A counter that is used to grab at least 10 reads before commiting the Home location
-    static uint8_t ground_start_count  = 10;
+    static uint32_t last_gps_reading;           // time of last gps message
+    static uint8_t ground_start_count = 10;     // counter used to grab at least 10 reads before commiting the Home location
 
     g_gps->update();
 
-    if (g_gps->new_data && last_gps_time != g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
-        // clear new data flag
-        g_gps->new_data = false;
+    // logging and glitch protection run after every gps message
+    if ((g_gps->last_message_time_ms() != last_gps_reading) && motors.armed()) {
+        last_gps_reading = g_gps->last_message_time_ms();
 
-        // save GPS time so we don't get duplicate reads
-        last_gps_time = g_gps->last_fix_time;
-
-        // log location if we have at least a 2D fix
-        if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
+        // log GPS message
+        if (g.log_bitmask & MASK_LOG_GPS) {
             DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
         }
 
+        // run glitch protection and update AP_Notify if home has been initialised
+        if (ap.home_is_set) {
+            gps_glitch.check_position();
+            if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
+                if (gps_glitch.glitching()) {
+                    Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
+                }else{
+                    Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
+                }
+                AP_Notify::flags.gps_glitching = gps_glitch.glitching();
+            }
+        }
+    }
+
+    // checks to initialise home and take location based pictures
+    if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
+        // clear new data flag
+        g_gps->new_data = false;
+
         // for performance monitoring
         gps_fix_count++;
 
-        // run glitch protection and update AP_Notify
-        gps_glitch.check_position();
-        if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
-            if (gps_glitch.glitching()) {
-                Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
-            }else{
-                Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
-            }
-            AP_Notify::flags.gps_glitching = gps_glitch.glitching();
-        }
-
         // check if we can initialise home yet
         if (!ap.home_is_set) {
             // if we have a 3d lock and valid location