From 8084a71c028066d3969b5c9668552a75d2a67c3c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Oct 2013 22:14:12 +1100 Subject: [PATCH] HAL_SITL: properly emulate timestamps on ublox and MTK --- libraries/AP_HAL_AVR_SITL/sitl_gps.cpp | 31 ++++++++++++++++---------- 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp index 0090a6cbe8..fdf19c3e2e 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -299,14 +299,17 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d) p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be - // seconds since the start of the day in UTC time, done in powers - // of 100. Quite bizarre. + // milliseconds since the start of the day in UTC time, + // done in powers of 100. + // The date is powers of 100 as well, but in days since 1/1/2000 struct tm tm; struct timeval tv; + gettimeofday(&tv, NULL); tm = *gmtime(&tv.tv_sec); + uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20 - p.utc_time = tm.tm_sec + tm.tm_min*100 + tm.tm_hour*100*100; + p.utc_time = hsec + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100; swap_uint32((uint32_t *)&p.latitude, 5); swap_uint32((uint32_t *)&p.utc_time, 1); @@ -353,16 +356,18 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d) p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be - // hundreadths of a second since the start of the day in UTC time, + // milliseconds since the start of the day in UTC time, // done in powers of 100. - // The data is powers of 100 as well, but in days since 1/1/2000 + // The date is powers of 100 as well, but in days since 1/1/2000 struct tm tm; struct timeval tv; + gettimeofday(&tv, NULL); tm = *gmtime(&tv.tv_sec); + uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20 - p.utc_date = (tm.tm_year-2000) + tm.tm_mon*100 + tm.tm_mday*100*100; - p.utc_time = tv.tv_usec/10000 + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100; + p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100); + p.utc_time = hsec + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100; p.hdop = 115; @@ -393,7 +398,7 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d) uint8_t ck_a; uint8_t ck_b; } p; - + p.preamble1 = 0xd1; p.preamble2 = 0xdd; p.size = sizeof(p) - 5; @@ -409,16 +414,18 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d) p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be - // hundreadths of a second since the start of the day in UTC time, + // milliseconds since the start of the day in UTC time, // done in powers of 100. - // The data is powers of 100 as well, but in days since 1/1/2000 + // The date is powers of 100 as well, but in days since 1/1/2000 struct tm tm; struct timeval tv; + gettimeofday(&tv, NULL); tm = *gmtime(&tv.tv_sec); + uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200 - p.utc_date = (tm.tm_year-2000) + tm.tm_mon*100 + tm.tm_mday*100*100; - p.utc_time = tv.tv_usec/10000 + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100; + p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100); + p.utc_time = millisec + tm.tm_sec*1000 + tm.tm_min*1000*100 + tm.tm_hour*1000*100*100; p.hdop = 115;