diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 6169ce69c2..9ce166704e 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -16,6 +16,8 @@ #include "AP_GPS.h" #include "GPS_Backend.h" #include +#include +#include #define GPS_BACKEND_DEBUGGING 0 @@ -76,34 +78,25 @@ int16_t AP_GPS_Backend::swap_int16(int16_t v) const */ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) { - uint8_t year, mon, day, hour, min, sec; - uint16_t msec; + struct tm tm {}; - year = bcd_date % 100; - mon = (bcd_date / 100) % 100; - day = bcd_date / 10000; + tm.tm_year = 100U + bcd_date % 100U; + tm.tm_mon = ((bcd_date / 100U) % 100U)-1; + tm.tm_mday = bcd_date / 10000U; uint32_t v = bcd_milliseconds; - msec = v % 1000; v /= 1000; - sec = v % 100; v /= 100; - min = v % 100; v /= 100; - hour = v % 100; + uint16_t msec = v % 1000U; v /= 1000U; + tm.tm_sec = v % 100U; v /= 100U; + tm.tm_min = v % 100U; v /= 100U; + tm.tm_hour = v % 100U; - int8_t rmon = mon - 2; - if (0 >= rmon) { - rmon += 12; - year -= 1; - } - - // get time in seconds since unix epoch - uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; - ret += year*365 + 10501; - ret = ret*24 + hour; - ret = ret*60 + min; - ret = ret*60 + sec; + // convert from time structure to unix time + time_t unix_time = AP::rtc().mktime(&tm); // convert to time since GPS epoch - ret -= 272764785UL; + const uint32_t unix_to_GPS_secs = 315964800UL; + const uint16_t leap_seconds_unix = GPS_LEAPSECONDS_MILLIS/1000U; + uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs; // get GPS week and time state.time_week = ret / AP_SEC_PER_WEEK;