AP_ExternalAHRS: fixed AdNav time handling

ArduPilot · Nov 10, 2023 · ff75167 · ff75167
1 parent 3466ba3
commit ff75167
Showing 1 changed file with 10 additions and 4 deletions.
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp
@@ -42,7 +42,6 @@
 #define AN_PACKET_ID_SYSTEM_STATE 20
 #define AN_PACKET_ID_DEVICE_INFO 3
 #define AN_PACKET_ID_ACKNOWLEDGE 0
-#define AN_GPS_EPOCH_UNIX_OFFSET 315964800 // GPS Week 0 sec 0 is midnight Sunday Jan 6th 1980 UTC
 #define AN_TIMEOUT 5000 //ms
 #define AN_MAXIMUM_PACKET_PERIODS 50
 
@@ -685,7 +684,8 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet()
 
         AP_ExternalAHRS::gps_data_message_t gps;
 
-        uint32_t sinceLastEpoch = _msg.packet.payload.raw_gnss.unix_time - AN_GPS_EPOCH_UNIX_OFFSET;
+        const uint32_t unix_sec = _msg.packet.payload.raw_gnss.unix_time;
+        const uint32_t unix_usec = _msg.packet.payload.raw_gnss.unix_microseconds;
         uint8_t fix = 0;
 
         switch (_msg.packet.payload.raw_gnss.flags.b.fix_type) {
@@ -715,8 +715,14 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet()
             break;
         }
 
-        gps.gps_week = (uint16_t) floor(sinceLastEpoch / AP_SEC_PER_WEEK);
-        gps.ms_tow = (uint32_t) (((sinceLastEpoch - (AP_SEC_PER_WEEK * gps.gps_week)) * 1.0e3) + (_msg.packet.payload.raw_gnss.unix_microseconds * 1.0e-3));
+        const uint32_t leapseconds = 18U;
+        const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - leapseconds;
+        const uint32_t epoch_seconds = unix_sec - epoch;
+        gps.gps_week = epoch_seconds / AP_SEC_PER_WEEK;
+        const uint32_t t_ms = unix_usec / 1000U;
+        // round time to nearest 200ms
+        gps.ms_tow = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200);
+
         gps.fix_type = fix;
         gps.satellites_in_view = (uint8_t) (_last_satellites->beidou_satellites + _last_satellites->galileo_satellites
                                             + _last_satellites->glonass_satellites + _last_satellites->gps_satellites + _last_satellites->sbas_satellites);