AP_GPS: make GPS packet timestamps more accurate

this reduces the effect of processing time and uart transmit time
ArduPilot · May 16, 2018 · 5fee158 · 5fee158
1 parent 62a73e4
commit 5fee158
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -842,6 +842,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         return false;
     }
 
+    // use the uart receive time to make packet timestamps more accurate
+    uint32_t last_uart_receive_ms = port->receive_time_constraint_us(state.payload_length + sizeof(ubx_header) + 2) / 1000;
+
     switch (_msg_id) {
     case MSG_POSLLH:
         Debug("MSG_POSLLH next_fix=%u", next_fix);
@@ -935,7 +938,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         }
         state.num_sats    = _buffer.solution.satellites;
         if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
-            state.last_gps_time_ms = AP_HAL::millis();
+            state.last_gps_time_ms = last_uart_receive_ms;
             state.time_week_ms    = _buffer.solution.time;
             state.time_week       = _buffer.solution.week;
         }
@@ -1013,7 +1016,7 @@ AP_GPS_UBLOX::_parse_gps(void)
             state.vdop        = _buffer.pvt.p_dop;
         }
 
-        state.last_gps_time_ms = AP_HAL::millis();
+        state.last_gps_time_ms = last_uart_receive_ms;
 
         // time
         state.time_week_ms    = _buffer.pvt.itow;