AP_GPS: 恢复为官方4.1的周秒计算

libraries/AP_GPS/GPS_Backend.cpp

@@ -16,8 +16,9 @@
 #include "AP_GPS.h"
 #include "GPS_Backend.h"
 #include <AP_Logger/AP_Logger.h>
-
+#include <AP_RTC/AP_RTC.h>
 #include <assert.h>
+#include <time.h>
 #define GPS_BACKEND_DEBUGGING 0
 
 #if GPS_BACKEND_DEBUGGING
@@ -77,44 +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;
-
-    time_t rawtime; 
-    struct tm* timeinfo;
-    time(&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]
-    
-     //convert to time since Unix epoch
-    uint32_t  ret =mkgmtime(timeinfo);
-    //GPS epoch
-    ret -= 315964800 + 18; //315964800   1980-1-6  - 1970 1- 1,+18s tiaomiao
-
-    // // 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 to time since GPS epoch
-    // ret -= 272764785UL;
+    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;
+
+    // convert from time structure to unix time
+    time_t unix_time = AP::rtc().mktime(&tm);
+
+    // convert to time since GPS epoch
+    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;
@@ -301,19 +283,3 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
     }
 }
 
-
-    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;
-}
-

libraries/AP_GPS/GPS_Backend.h

@@ -20,8 +20,6 @@
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_RTC/JitterCorrection.h>
-#include <time.h>
-#include <sys/time.h>
 #include "AP_GPS.h"
 
 class AP_GPS_Backend
@@ -99,7 +97,6 @@ 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;

libraries/AP_RTC/AP_RTC.h

@@ -3,7 +3,7 @@
 #include <AP_Param/AP_Param.h>
 
 #include <stdint.h>
-
+#include <time.h>
 class AP_RTC {
 
 public: