From d34e5b3f429acfbd0d12de1257e31108e84db9c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Jan 2015 08:36:15 +1100 Subject: [PATCH] AP_GPS: fixed PX4 GPS driver for new upstream format --- libraries/AP_GPS/AP_GPS_PX4.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_PX4.cpp b/libraries/AP_GPS/AP_GPS_PX4.cpp index 660ca0d237..e59fd82a28 100644 --- a/libraries/AP_GPS/AP_GPS_PX4.cpp +++ b/libraries/AP_GPS/AP_GPS_PX4.cpp @@ -67,12 +67,12 @@ AP_GPS_PX4::read(void) // convert epoch timestamp back to gps epoch - evil hack until we get the genuine // raw week information (or APM switches to Posix epoch ;-) ) - uint64_t epoch_ms = uint64_t(_gps_pos.time_gps_usec/1000.+.5); + uint64_t epoch_ms = uint64_t(_gps_pos.time_utc_usec/1000.+.5); uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014; state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK)); state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK); - if (_gps_pos.time_gps_usec == 0) { + if (_gps_pos.time_utc_usec == 0) { // This is a work-around for https://github.com/PX4/Firmware/issues/1474 // reject position reports with invalid time, as APM adjusts it's clock after the first lock has been aquired state.status = AP_GPS::NO_FIX;