From 3fe4b9c0f8310c3ad6c48ca5f98749a6bd8cd070 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 May 2018 11:16:01 +1000 Subject: [PATCH] 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 | 8 +++++++- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_ERB.cpp | 2 ++ libraries/AP_GPS/AP_GPS_NMEA.cpp | 5 +++++ libraries/AP_GPS/AP_GPS_NMEA.h | 1 + libraries/AP_GPS/AP_GPS_SBF.cpp | 1 + libraries/AP_GPS/AP_GPS_SBP2.cpp | 1 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 20 +++++++++++++++++--- libraries/AP_GPS/AP_GPS_UBLOX.h | 14 +++++++++----- libraries/AP_GPS/GPS_Backend.cpp | 11 +++++++++++ libraries/AP_GPS/GPS_Backend.h | 6 ++++++ 11 files changed, 61 insertions(+), 9 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index ff146e3610..1ed7f68db7 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 78b581e3c8..758c2ed430 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index 90a7399fde..979bda5072 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index e6524f50a6..2b4661170b 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 712410efb9..31c35e207e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 8639cc2504..eb99f406ec 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/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) diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index 56ae3dd106..964a711b91 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/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; // diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 735079a20b..f67a5a1155 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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); + } +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 522aa0777d..293b073ff9 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/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); diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index a129c4dbc5..b903e7858a 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/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; + } +} diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 3b9c740524..927f86e0ad 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/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); };