From 4d990fa6c87d398d4a5c7195ed1d1b25fdfb8507 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 10 Apr 2014 10:29:53 +1000
Subject: [PATCH] Plane: ensure we log both GPS at the time we receive a msg

---
 ArduPlane/ArduPlane.pde | 12 +++++++-----
 ArduPlane/Log.pde       |  6 +++---
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index a483b9b18d..d4f9b65bfb 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -1009,13 +1009,15 @@ static void airspeed_ratio_update(void)
  */
 static void update_GPS_50Hz(void)
 {
-    static uint32_t last_gps_reading;
+    static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
     gps.update();
 
-    if (gps.last_message_time_ms() != last_gps_reading) {
-        last_gps_reading = gps.last_message_time_ms();
-        if (should_log(MASK_LOG_GPS)) {
-            Log_Write_GPS();
+    for (uint8_t i=0; i<gps.num_sensors(); i++) {
+        if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
+            last_gps_reading[i] = gps.last_message_time_ms(i);
+            if (should_log(MASK_LOG_GPS)) {
+                Log_Write_GPS(i);
+            }
         }
     }
 }
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index fd25a71ba1..5f10593f09 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -491,9 +491,9 @@ static void Log_Write_Compass()
 #endif
 }
 
-static void Log_Write_GPS(void)
+static void Log_Write_GPS(uint8_t instance)
 {
-    DataFlash.Log_Write_GPS(gps, current_loc.alt - ahrs.get_home().alt);
+    DataFlash.Log_Write_GPS(gps, instance, current_loc.alt - ahrs.get_home().alt);
 }
 
 static void Log_Write_IMU() 
@@ -612,7 +612,7 @@ static void Log_Write_Control_Tuning() {}
 static void Log_Write_Camera() {}
 static void Log_Write_Mode(uint8_t mode) {}
 static void Log_Write_Compass() {}
-static void Log_Write_GPS() {}
+static void Log_Write_GPS(uint8_t instance) {}
 static void Log_Write_IMU() {}
 static void Log_Write_RC() {}
 static void Log_Write_Airspeed(void) {}