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

@@ -603,7 +603,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
@@ -632,6 +632,12 @@ void AP_GPS::update_instance(uint8_t instance)
             data_should_be_logged = true;
         }
     } else {
+        if (state[instance].uart_timestamp_ms != 0) {
+            // set the timestamp for this messages based on
+            // set_uart_timestamp() in backend, if available
+            tnow = state[instance].uart_timestamp_ms;
+            state[instance].uart_timestamp_ms = 0;
+        }
         // 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

@@ -148,6 +148,7 @@ public:
         bool have_horizontal_accuracy;    ///< does GPS give horizontal position accuracy? Set to true only once available.
         bool have_vertical_accuracy;      ///< 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
+        uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
 
         // 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

@@ -178,6 +178,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
+            set_uart_timestamp(_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

@@ -99,6 +99,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
@@ -107,6 +108,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;
     }
 
@@ -116,6 +118,8 @@ bool AP_GPS_NMEA::_decode(char c)
     if (!_is_checksum_term)
         _parity ^= c;
 
+    _sentence_length++;
+    
     return valid_sentence;
 }
 
@@ -252,6 +256,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);
+                    set_uart_timestamp(_sentence_length);
                     state.last_gps_time_ms = now;
                     fill_3d_velocity();
                     break;

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -125,6 +125,7 @@ private:
     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);
         }
 
+        set_uart_timestamp(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;
+        set_uart_timestamp(parser_state.msg_len);
         state.last_gps_time_ms = now;
 
         //

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -833,7 +833,8 @@ AP_GPS_UBLOX::_parse_gps(void)
             _unconfigured_messages |= CONFIG_RATE_POSLLH;
             break;
         }
-        _last_pos_time        = _buffer.posllh.time;
+        _check_new_itow(_buffer.posllh.itow);
+        _last_pos_time        = _buffer.posllh.itow;
         state.location.lng    = _buffer.posllh.longitude;
         state.location.lat    = _buffer.posllh.latitude;
         state.location.alt    = _buffer.posllh.altitude_msl / 10;
@@ -883,6 +884,7 @@ AP_GPS_UBLOX::_parse_gps(void)
     case MSG_DOP:
         Debug("MSG_DOP");
         noReceivedHdop = false;
+        _check_new_itow(_buffer.dop.itow);
         state.hdop        = _buffer.dop.hDOP;
         state.vdop        = _buffer.dop.vDOP;
 #if UBLOX_FAKE_3DLOCK
@@ -894,6 +896,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_SOL fix_status=%u fix_type=%u",
               _buffer.solution.fix_status,
               _buffer.solution.fix_type);
+        _check_new_itow(_buffer.solution.itow);
         if (havePvtMsg) {
             state.time_week = _buffer.solution.week;
             break;
@@ -920,7 +923,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.time_week_ms    = _buffer.solution.time;
+            state.time_week_ms    = _buffer.solution.itow;
             state.time_week       = _buffer.solution.week;
         }
 #if UBLOX_FAKE_3DLOCK
@@ -936,6 +939,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_PVT");
         havePvtMsg = true;
         // position
+        _check_new_itow(_buffer.pvt.itow);
         _last_pos_time        = _buffer.pvt.itow;
         state.location.lng    = _buffer.pvt.lon;
         state.location.lat    = _buffer.pvt.lat;
@@ -1023,7 +1027,8 @@ AP_GPS_UBLOX::_parse_gps(void)
             _unconfigured_messages |= CONFIG_RATE_VELNED;
             break;
         }
-        _last_vel_time         = _buffer.velned.time;
+        _check_new_itow(_buffer.velned.itow);
+        _last_vel_time         = _buffer.velned.itow;
         state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
         state.ground_course    = wrap_360(_buffer.velned.heading_2d * 1.0e-5f);       // Heading 2D deg * 100000
         state.have_vertical_velocity = true;
@@ -1043,6 +1048,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         {
         Debug("MSG_NAV_SVINFO\n");
         static const uint8_t HardwareGenerationMask = 0x07;
+        _check_new_itow(_buffer.svinfo_header.itow);
         _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
         switch (_hardware_generation) {
             case UBLOX_5:
@@ -1329,3 +1335,11 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const
                                            _version.swVersion);
     }
 }
+
+void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
+{
+    if (itow != _last_itow) {
+        _last_itow = itow;
+        set_uart_timestamp(_payload_length + sizeof(ubx_header) + 2);
+    }
+}

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -193,7 +193,7 @@ private:
         uint32_t scanmode1;
     };
     struct PACKED ubx_nav_posllh {
-        uint32_t time;                                  // GPS msToW
+        uint32_t itow;                                  // GPS msToW
         int32_t longitude;
         int32_t latitude;
         int32_t altitude_ellipsoid;
@@ -202,7 +202,7 @@ private:
         uint32_t vertical_accuracy;
     };
     struct PACKED ubx_nav_status {
-        uint32_t time;                                  // GPS msToW
+        uint32_t itow;                                  // GPS msToW
         uint8_t fix_type;
         uint8_t fix_status;
         uint8_t differential_status;
@@ -211,7 +211,7 @@ private:
         uint32_t uptime;                                // milliseconds
     };
     struct PACKED ubx_nav_dop {
-        uint32_t time;                                  // GPS msToW
+        uint32_t itow;                                  // GPS msToW
         uint16_t gDOP;
         uint16_t pDOP;
         uint16_t tDOP;
@@ -221,7 +221,7 @@ private:
         uint16_t eDOP;
     };
     struct PACKED ubx_nav_solution {
-        uint32_t time;
+        uint32_t itow;
         int32_t time_nsec;
         uint16_t week;
         uint8_t fix_type;
@@ -263,7 +263,7 @@ private:
         uint8_t reserved2[4]; 
     };
     struct PACKED ubx_nav_velned {
-        uint32_t time;                                  // GPS msToW
+        uint32_t itow;                                  // GPS msToW
         int32_t ned_north;
         int32_t ned_east;
         int32_t ned_down;
@@ -549,6 +549,9 @@ private:
     
     bool havePvtMsg;
 
+    // itow from previous message
+    uint32_t _last_itow;
+
     bool        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
     void        _configure_rate(void);
     void        _configure_sbas(bool enable);
@@ -561,6 +564,7 @@ private:
     void        _request_version(void);
     void        _save_cfg(void);
     void        _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+    void        _check_new_itow(uint32_t itow);
 
     void unexpected_message(void);
     void log_mon_hw(void);

libraries/AP_GPS/GPS_Backend.cpp

@@ -216,3 +216,14 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
     }
 }
 
+
+/*
+  set a timestamp based on arrival time on uart at current byte,
+  assuming the message started nbytes ago
+*/
+void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
+{
+    if (port) {
+        state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
+    }
+}

libraries/AP_GPS/GPS_Backend.h

@@ -87,4 +87,10 @@ protected:
     void _detection_message(char *buffer, uint8_t buflen) const;
 
     bool should_df_log() const;
+
+    /*
+      set a timestamp based on arrival time on uart at current byte,
+      assuming the message started nbytes ago
+     */
+    void set_uart_timestamp(uint16_t nbytes);
 };