HAL_SITL: fixed GPS rate in SITL when speedup used

libraries/AP_HAL_SITL/sitl_gps.cpp

@@ -125,6 +125,25 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size)
     }
 }
 
+/*
+  get timeval using simulation time
+ */
+static void simulation_timeval(struct timeval *tv)
+{
+    uint64_t now = AP_HAL::micros64();
+    static uint64_t first_usec;
+    static struct timeval first_tv;
+    if (first_usec == 0) {
+        first_usec = now;
+        gettimeofday(&first_tv, NULL);
+    }
+    *tv = first_tv;
+    tv->tv_sec += now / 1000000ULL;
+    uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
+    tv->tv_sec += new_usec / 1000000ULL;
+    tv->tv_usec = new_usec % 1000000ULL;
+}
+
 /*
   send a UBLOX GPS message
  */
@@ -158,7 +177,7 @@ void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
 static void gps_time(uint16_t *time_week, uint32_t *time_week_ms)
 {
     struct timeval tv;
-    gettimeofday(&tv, NULL);
+    simulation_timeval(&tv);
     const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - 15;
     uint32_t epoch_seconds = tv.tv_sec - epoch;
     *time_week = epoch_seconds / (86400*7UL);
@@ -357,7 +376,7 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d)
     struct tm tm;
     struct timeval tv;
 
-    gettimeofday(&tv, NULL);
+    simulation_timeval(&tv);
     tm = *gmtime(&tv.tv_sec);
     uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20
 
@@ -414,7 +433,7 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d)
     struct tm tm;
     struct timeval tv;
 
-    gettimeofday(&tv, NULL);
+    simulation_timeval(&tv);
     tm = *gmtime(&tv.tv_sec);
     uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200
 
@@ -472,7 +491,7 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d)
     struct tm tm;
     struct timeval tv;
 
-    gettimeofday(&tv, NULL);
+    simulation_timeval(&tv);
     tm = *gmtime(&tv.tv_sec);
     uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200
 
@@ -533,7 +552,7 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
     char lat_string[20];
     char lng_string[20];
 
-    gettimeofday(&tv, NULL);
+    simulation_timeval(&tv);
 
     tm = gmtime(&tv.tv_sec);