Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: fixed GPS rate in SITL when speedup used

This commit is contained in:
Andrew Tridgell 2016-05-04 08:22:55 +10:00
parent 8a987bf67d
commit 2718b0649b
1 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -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);