diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-04 10:45:00 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-04 10:45:20 +0100 |
commit | f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e (patch) | |
tree | 5e246b2a560a459902561df5a9cf5a83c400aca8 /src/drivers/gps | |
parent | 50a00bee8ed56d41d1c3e74eaaa7997191793595 (diff) | |
download | px4-firmware-f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e.tar.gz px4-firmware-f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e.tar.bz2 px4-firmware-f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e.zip |
GPS driver: Set RTC from Ashtech receivers as well
Diffstat (limited to 'src/drivers/gps')
-rw-r--r-- | src/drivers/gps/ashtech.cpp | 17 |
1 files changed, 15 insertions, 2 deletions
diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a2e292de2..c2381b8dc 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,8 +99,21 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_gps_usec = static_cast<uint64_t>(epoch) * 1000000ULL; + _gps_position->time_gps_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } |