diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 6f9f5178d4..c219621f8a 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/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);