|
|
|
@ -16,6 +16,8 @@
@@ -16,6 +16,8 @@
|
|
|
|
|
#include "AP_GPS.h" |
|
|
|
|
#include "GPS_Backend.h" |
|
|
|
|
#include <AP_Logger/AP_Logger.h> |
|
|
|
|
#include <time.h> |
|
|
|
|
#include <AP_RTC/AP_RTC.h> |
|
|
|
|
|
|
|
|
|
#define GPS_BACKEND_DEBUGGING 0 |
|
|
|
|
|
|
|
|
@ -76,34 +78,25 @@ int16_t AP_GPS_Backend::swap_int16(int16_t v) const
@@ -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; |
|
|
|
|
|
|
|
|
|
int8_t rmon = mon - 2; |
|
|
|
|
if (0 >= rmon) {
|
|
|
|
|
rmon += 12; |
|
|
|
|
year -= 1; |
|
|
|
|
} |
|
|
|
|
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; |
|
|
|
|
|
|
|
|
|
// 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; |
|
|
|
|