From 1cc92c03612c504414092bc6742bb17c812ce9c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:43 +0100 Subject: GPS driver: Require valid minimum time to allow setting the wall clock. Protection against nulled time fields --- src/drivers/gps/ashtech.cpp | 29 +++++++++++++---------- src/drivers/gps/gps_helper.h | 2 ++ src/drivers/gps/ubx.cpp | 56 ++++++++++++++++++++++++++------------------ 3 files changed, 52 insertions(+), 35 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 99dbe5984..a50767c22 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,21 +99,26 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - uint64_t usecs = static_cast((ashtech_sec - static_cast(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. + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast((ashtech_sec - static_cast(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"); + } - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 3623397b2..0ce05fcb5 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,8 @@ #include #include +#define GPS_EPOCH_SECS 1234567890ULL + class GPS_Helper { public: diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 05809b361..e197059db 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,19 +748,23 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); - // 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 = _buf.payload_rx_nav_pvt.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // 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 = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); @@ -820,19 +824,25 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // 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. + // only set the time if it makes sense - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // 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. - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); -- cgit v1.2.3