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