AP_GPS: make GPS packet timestamps more accurate

this reduces the effect of processing time and uart transmit time

libraries/AP_GPS/AP_GPS.cpp

@@ -635,7 +635,7 @@ void AP_GPS::update_instance(uint8_t instance)
 
     // we have an active driver for this instance
     bool result = drivers[instance]->read();
-    const uint32_t tnow = AP_HAL::millis();
+    uint32_t tnow = AP_HAL::millis();
 
     // if we did not get a message, and the idle timer of 2 seconds
     // has expired, re-initialise the GPS. This will cause GPS
@@ -664,6 +664,13 @@ void AP_GPS::update_instance(uint8_t instance)
             data_should_be_logged = true;
         }
     } else {
+        if (_port[instance]) {
+            // if possible constrain time based on uart receive time
+            uint64_t timestamp_us = _port[instance]->receive_time_constraint_us(state[instance].message_length);
+            if (timestamp_us != 0) {
+                tnow = timestamp_us / 1000U;
+            }
+        }
         // delta will only be correct after parsing two messages
         timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;
         timing[instance].last_message_time_ms = tnow;

libraries/AP_GPS/AP_GPS.h

@@ -149,6 +149,7 @@ class AP_GPS
         bool have_horizontal_accuracy:1;    ///< does GPS give horizontal position accuracy? Set to true only once available.
         bool have_vertical_accuracy:1;      ///< does GPS give vertical position accuracy? Set to true only once available.
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
+        uint16_t message_length;            ///< optional message length for uart latency calculations
 
         // all the following fields must only all be filled by RTK capable backend drivers
         uint32_t rtk_time_week_ms;         ///< GPS Time of Week of last baseline in milliseconds

libraries/AP_GPS/AP_GPS_ERB.cpp

@@ -185,6 +185,8 @@ AP_GPS_ERB::_parse_gps(void)
         }
         state.num_sats = _buffer.stat.satellites;
         if (next_fix >= AP_GPS::GPS_OK_FIX_3D) {
+            // use the uart receive time to make packet timestamps more accurate
+            state.message_length = _payload_length + sizeof(erb_header) + 2;
             state.last_gps_time_ms = AP_HAL::millis();
             state.time_week_ms    = _buffer.stat.time;
             state.time_week       = _buffer.stat.week;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -105,6 +105,7 @@ bool AP_GPS_NMEA::_decode(char c)
         ++_term_number;
         _term_offset = 0;
         _is_checksum_term = c == '*';
+        _sentence_length++;
         return valid_sentence;
 
     case '$': // sentence begin
@@ -113,6 +114,7 @@ bool AP_GPS_NMEA::_decode(char c)
         _sentence_type = _GPS_SENTENCE_OTHER;
         _is_checksum_term = false;
         _gps_data_good = false;
+        _sentence_length = 0;
         return valid_sentence;
     }
 
@@ -122,6 +124,8 @@ bool AP_GPS_NMEA::_decode(char c)
     if (!_is_checksum_term)
         _parity ^= c;
 
+    _sentence_length++;
+
     return valid_sentence;
 }
 
@@ -258,6 +262,7 @@ bool AP_GPS_NMEA::_term_complete()
                     state.ground_speed     = _new_speed*0.01f;
                     state.ground_course    = wrap_360(_new_course*0.01f);
                     make_gps_time(_new_date, _new_time * 10);
+                    state.message_length = _sentence_length;
                     state.last_gps_time_ms = now;
                     fill_3d_velocity();
                     break;

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -125,6 +125,7 @@ class AP_GPS_NMEA : public AP_GPS_Backend
     uint8_t _sentence_type;                                     ///< the sentence type currently being processed
     uint8_t _term_number;                                       ///< term index within the current sentence
     uint8_t _term_offset;                                       ///< character offset with the term being received
+    uint16_t _sentence_length;
     bool _gps_data_good;                                        ///< set when the sentence indicates data is good
 
     // The result of parsing terms within a message is stored temporarily until

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -284,6 +284,7 @@ AP_GPS_SBF::process_message(void)
             state.time_week_ms = (uint32_t)(temp.TOW);
         }
 
+        state.message_length = sbf_msg.length;
         state.last_gps_time_ms = AP_HAL::millis();
 
         // Update velocity state (don't use −2·10^10)

libraries/AP_GPS/AP_GPS_SBP2.cpp

@@ -288,6 +288,7 @@ AP_GPS_SBP2::_attempt_state_update()
         state.time_week_ms      = last_vel_ned.tow;
         state.hdop              = last_dops.hdop;
         state.vdop              = last_dops.vdop;
+        state.message_length = parser_state.msg_len;
         state.last_gps_time_ms = now;
 
         //

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -859,6 +859,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
         state.have_horizontal_accuracy = true;
         state.have_vertical_accuracy = true;
+        state.message_length = _payload_length + sizeof(ubx_header) + 2;        
 #if UBLOX_FAKE_3DLOCK
         state.location.lng = 1491652300L;
         state.location.lat = -353632610L;
@@ -1014,6 +1015,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         }
 
         state.last_gps_time_ms = AP_HAL::millis();
+        state.message_length = _payload_length + sizeof(ubx_header) + 2;
 
         // time
         state.time_week_ms    = _buffer.pvt.itow;