diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index f453f02591..c51c7d5c14 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -16,8 +16,8 @@ #include "AP_GPS.h" #include "GPS_Backend.h" #include -#include -#include + +#include #define GPS_BACKEND_DEBUGGING 0 #if GPS_BACKEND_DEBUGGING @@ -99,15 +99,18 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) time_t rawtime; struct tm* timeinfo; time(&rawtime); - timeinfo = localtime(&rawtime); + timeinfo = gmtime(&rawtime); timeinfo->tm_year = year+2000 - 1900; timeinfo->tm_mon = mon - 1; //months since January - [0,11] timeinfo->tm_mday = day; //day of the month - [1,31] timeinfo->tm_hour = hour; //hours since midnight - [0,23] timeinfo->tm_min = min; //minutes after the hour - [0,59] timeinfo->tm_sec = sec; //seconds after the minute - [0,59] - uint32_t ret = mktime(timeinfo); - ret -= 315936000; //315936000 1980-1-6 to 1970 1- 1 + + //convert to time since Unix epoch + uint32_t ret =mkgmtime(timeinfo); + //GPS epoch + ret -= 315964800; //315964800 1980-1-6 - 1970 1- 1 // // get time in seconds since unix epoch // uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; @@ -303,3 +306,20 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) state.uart_timestamp_ms = local_us / 1000U; } } + + + time_t AP_GPS_Backend::mkgmtime(struct tm *unixdate) +{ + assert(unixdate != nullptr); + + time_t fakeUnixTime = mktime(unixdate); + struct tm *fakeDate = gmtime(&fakeUnixTime); + + int32_t nOffset = fakeDate->tm_hour - unixdate->tm_hour; + if (nOffset > 12) + { + nOffset = 24 - nOffset; + } + return fakeUnixTime - nOffset * 3600; +} + diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 973d7611aa..7c7a9329f3 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include "AP_GPS.h" class AP_GPS_Backend @@ -97,6 +99,7 @@ protected: void check_new_itow(uint32_t itow, uint32_t msg_length); + time_t mkgmtime(struct tm *unixdate); private: // itow from previous message uint32_t _last_itow;