AP_GPS: move to using last_corrected_gps_time_us instead of uart_timestamp_us

libraries/AP_GPS/AP_GPS.cpp

@@ -879,11 +879,12 @@ void AP_GPS::update_instance(uint8_t instance)
             data_should_be_logged = true;
         }
     } else {
-        if (state[instance].uart_timestamp_ms != 0) {
+        if (state[instance].corrected_timestamp_updated) {
             // set the timestamp for this messages based on
-            // set_uart_timestamp() in backend, if available
+            // set_uart_timestamp() or per specific transport in backend
-            tnow = state[instance].uart_timestamp_ms;
+            // , if available
-            state[instance].uart_timestamp_ms = 0;
+            tnow = state[instance].last_corrected_gps_time_us/1000U;
+            state[instance].corrected_timestamp_updated = false;
         }
         // delta will only be correct after parsing two messages
         timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;

libraries/AP_GPS/AP_GPS.h

@@ -194,8 +194,8 @@ public:
         bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
         bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
-        uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
         uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
+        bool corrected_timestamp_updated;  ///< true if the corrected timestamp has been updated
         uint32_t lagged_sample_count;       ///< number of samples with 50ms more lag than expected
 
         // all the following fields must only all be filled by RTK capable backend drivers

libraries/AP_GPS/AP_GPS_MAV.cpp

@@ -121,7 +121,8 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 }
                 uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
                 uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
-                state.uart_timestamp_ms = corrected_ms;
+                state.last_corrected_gps_time_us = (corrected_ms * 1000ULL);
+                state.corrected_timestamp_updated = true;
                 if (have_yaw) {
                     state.gps_yaw_time_ms = corrected_ms;
                 }

libraries/AP_GPS/GPS_Backend.cpp

@@ -227,7 +227,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
 void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
 {
     if (port) {
-        state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
+        state.last_corrected_gps_time_us = port->receive_time_constraint_us(nbytes);
+        state.corrected_timestamp_updated = true;
     }
 }
 
@@ -291,14 +292,14 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
         // use msg arrival time, and correct for jitter
         uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
         state.last_corrected_gps_time_us = local_us;
-        state.uart_timestamp_ms = local_us / 1000U;
+        state.corrected_timestamp_updated = true;
 
         // look for lagged data from the GPS. This is meant to detect
         // the case that the GPS is trying to push more data into the
         // UART than can fit (eg. with GPS_RAW_DATA at 115200).
         float expected_lag;
         if (gps.get_lag(state.instance, expected_lag)) {
-            float lag_s = (now - state.uart_timestamp_ms) * 0.001;
+            float lag_s = (now - (state.last_corrected_gps_time_us/1000U)) * 0.001;
             if (lag_s > expected_lag+0.05) {
                 // more than 50ms over expected lag, increment lag counter
                 state.lagged_sample_count++;