diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index b59f645b38..78e8c11c46 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -48,8 +48,7 @@ public:
     void EnableWrites(bool enable) { _writes_enabled = enable; }
     void Log_Write_Format(const struct LogStructure *structure);
     void Log_Write_Parameter(const char *name, float value);
-    void Log_Write_GPS(const GPS *gps, int32_t relative_alt);
-    void Log_Write_GPS2(const GPS *gps);
+    void Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt);
     void Log_Write_IMU(const AP_InertialSensor &ins);
     void Log_Write_RCIN(void);
     void Log_Write_RCOUT(void);
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index f1979d57ff..036ad7b607 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -662,48 +662,49 @@ void DataFlash_Class::Log_Write_Parameters(void)
 
 
 // Write an GPS packet
-void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt)
+void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt)
 {
+    const struct Location &loc = gps.location(0);
     struct log_GPS pkt = {
         LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
-    	status        : (uint8_t)gps->status(),
-    	gps_week_ms   : gps->time_week_ms,
-    	gps_week      : gps->time_week,
-        num_sats      : gps->num_sats,
-        hdop          : gps->hdop,
-        latitude      : gps->latitude,
-        longitude     : gps->longitude,
+    	status        : (uint8_t)gps.status(0),
+    	gps_week_ms   : gps.time_week_ms(0),
+    	gps_week      : gps.time_week(0),
+        num_sats      : gps.num_sats(0),
+        hdop          : gps.get_hdop(0),
+        latitude      : loc.lat,
+        longitude     : loc.lng,
         rel_altitude  : relative_alt,
-        altitude      : gps->altitude_cm,
-        ground_speed  : gps->ground_speed_cm,
-        ground_course : gps->ground_course_cd,
-        vel_z         : gps->velocity_down(),
+        altitude      : loc.alt,
+        ground_speed  : (uint32_t)(gps.ground_speed(0) * 100),
+        ground_course : gps.ground_course_cd(0),
+        vel_z         : gps.velocity(0).z,
         apm_time      : hal.scheduler->millis()
     };
     WriteBlock(&pkt, sizeof(pkt));
-}
-
-// Write a GPS2 packet
-void DataFlash_Class::Log_Write_GPS2(const GPS *gps)
-{
-    struct log_GPS2 pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG),
-    	status        : (uint8_t)gps->status(),
-    	gps_week_ms   : gps->time_week_ms,
-    	gps_week      : gps->time_week,
-        num_sats      : gps->num_sats,
-        hdop          : gps->hdop,
-        latitude      : gps->latitude,
-        longitude     : gps->longitude,
-        altitude      : gps->altitude_cm,
-        ground_speed  : gps->ground_speed_cm,
-        ground_course : gps->ground_course_cd,
-        vel_z         : gps->velocity_down(),
-        apm_time      : hal.scheduler->millis(),
-        dgps_numch    : 0,
-        dgps_age      : 0
-    };
-    WriteBlock(&pkt, sizeof(pkt));
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
+    if (gps.num_sensors() > 1) {
+        const struct Location &loc2 = gps.location(1);
+        struct log_GPS2 pkt2 = {
+            LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG),
+            status        : (uint8_t)gps.status(1),
+            gps_week_ms   : gps.time_week_ms(1),
+            gps_week      : gps.time_week(1),
+            num_sats      : gps.num_sats(1),
+            hdop          : gps.get_hdop(1),
+            latitude      : loc2.lat,
+            longitude     : loc2.lng,
+            altitude      : loc2.alt,
+            ground_speed  : (uint32_t)(gps.ground_speed(1)*100),
+            ground_course : gps.ground_course_cd(1),
+            vel_z         : gps.velocity(1).z,
+            apm_time      : hal.scheduler->millis(),
+            dgps_numch    : 0,
+            dgps_age      : 0
+        };
+        WriteBlock(&pkt2, sizeof(pkt2));
+    }
+#endif
 }
 
 // Write an RCIN packet