Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: improve time field handling in simulated GPS

Browse Source
This commit is contained in:
Andrew Tridgell 2013-02-16 21:36:06 +11:00
parent 428966160a
commit 3a582663fb
1 changed files with 58 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -24,6 +24,7 @@
#include <sys/ioctl.h>
#include <unistd.h>
#include <time.h>
#include <sys/time.h>
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);