From 3a582663fb2d6928c14e408ebcd037616d041541 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Feb 2013 21:36:06 +1100 Subject: [PATCH] SITL: improve time field handling in simulated GPS --- libraries/AP_HAL_AVR_SITL/sitl_gps.cpp | 67 ++++++++++++++++++++++---- 1 file changed, 58 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp index 18405fae62..3484935493 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -24,6 +24,7 @@ #include #include #include +#include using namespace AVR_SITL; extern const AP_HAL::HAL& hal; @@ -114,6 +115,23 @@ static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) write(gps_state.gps_fd, chk, sizeof(chk)); } +/* + return GPS time of week in milliseconds + */ +static uint32_t millis_time_of_week(void) +{ + struct tm tm; + struct timeval tv; + gettimeofday(&tv, NULL); + tm = *gmtime(&tv.tv_sec); + uint32_t tsec; + tsec = + tm.tm_wday * 24 * 3600 + + tm.tm_hour * 3600 + + tm.tm_min * 60 + + tm.tm_sec; + return tsec + (tv.tv_usec/1000); +} /* send a new set of GPS UBLOX packets @@ -183,7 +201,7 @@ static void _update_gps_ubx(const struct gps_data *d) pos.horizontal_accuracy = 5; pos.vertical_accuracy = 10; - status.time = pos.time; + status.time = millis_time_of_week(); status.fix_type = d->have_lock?3:0; status.fix_status = d->have_lock?1:0; status.differential_status = 0; @@ -191,7 +209,7 @@ static void _update_gps_ubx(const struct gps_data *d) status.time_to_first_fix = 0; status.uptime = hal.scheduler->millis(); - velned.time = pos.time; + velned.time = status.time; velned.ned_north = 100.0 * d->speedN; velned.ned_east = 100.0 * d->speedE; velned.ned_down = 0; @@ -274,7 +292,16 @@ static void _update_gps_mtk(const struct gps_data *d) } p.satellites = d->have_lock?10:3; p.fix_type = d->have_lock?3:1; - p.utc_time = time(NULL); + + // 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. + struct tm tm; + struct timeval tv; + gettimeofday(&tv, NULL); + tm = *gmtime(&tv.tv_sec); + + p.utc_time = tm.tm_sec + tm.tm_min*100 + tm.tm_hour*100*100; swap_uint32((uint32_t *)&p.latitude, 5); swap_uint32((uint32_t *)&p.utc_time, 1); @@ -321,9 +348,20 @@ static void _update_gps_mtk16(const struct gps_data *d) } p.satellites = d->have_lock?10:3; p.fix_type = d->have_lock?3:1; - p.utc_date = time(NULL); - p.utc_time = time(NULL); - p.hdop = 0; + + // 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, + // done in powers of 100. + // The data 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); + + 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.hdop = 115; mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b); @@ -368,9 +406,20 @@ static void _update_gps_mtk19(const struct gps_data *d) } p.satellites = d->have_lock?10:3; p.fix_type = d->have_lock?3:1; - p.utc_date = time(NULL); - p.utc_time = time(NULL); - p.hdop = 0; + + // 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, + // done in powers of 100. + // The data 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); + + 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.hdop = 115; mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);