From 9834304525caf7bb36eb7c8bab62506571abe5a7 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 16 Jan 2022 17:45:59 +0530 Subject: [PATCH] AP_GPS: make more precise calculation of gps message use PPS signal if available as well --- libraries/AP_GPS/AP_GPS.cpp | 26 ++++++++++++++++++++++---- libraries/AP_GPS/AP_GPS.h | 6 ++++++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 14 ++++++++++++++ libraries/AP_GPS/AP_GPS_UBLOX.h | 4 ++++ libraries/AP_GPS/GPS_Backend.cpp | 10 +++++++++- libraries/AP_GPS/GPS_Backend.h | 6 +++++- libraries/AP_RTC/JitterCorrection.h | 4 +++- 7 files changed, 63 insertions(+), 7 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index dc4161fa74..8786e96596 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -513,12 +513,30 @@ uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const { const GPS_State &istate = state[instance]; - if (istate.last_gps_time_ms == 0 || istate.time_week == 0) { + if ((istate.last_gps_time_ms == 0 && istate.last_corrected_gps_time_us == 0) || istate.time_week == 0) { return 0; } - uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms); - // add in the milliseconds since the last fix - return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; + uint64_t fix_time_ms; + // add in the time since the last fix message + if (istate.last_corrected_gps_time_us != 0) { + fix_time_ms = time_epoch_convert(istate.time_week, drivers[instance]->get_last_itow()); + return (fix_time_ms*1000ULL) + (AP_HAL::micros64() - istate.last_corrected_gps_time_us); + } else { + fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms) * 1000ULL; + return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; + } +} + +/** + calculate last message time since the unix epoch in microseconds + */ +uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const +{ + const GPS_State &istate = state[instance]; + if (istate.time_week == 0) { + return 0; + } + return time_epoch_convert(istate.time_week, drivers[instance]->get_last_itow()) * 1000ULL; } /* diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a199d4d925..c1845df901 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -195,6 +195,7 @@ public: 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 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 @@ -478,6 +479,11 @@ public: return time_epoch_usec(primary_instance); } + uint64_t last_message_epoch_usec(uint8_t instance) const; + uint64_t last_message_epoch_usec() const { + return last_message_epoch_usec(primary_instance); + } + // convert GPS week and millis to unix epoch in ms static uint64_t time_epoch_convert(uint16_t gps_week, uint32_t gps_ms); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 4ff67aa0f8..dbb4012a79 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1055,6 +1055,9 @@ AP_GPS_UBLOX::_parse_gps(void) (unsigned)_buffer.nav_tp5.version, (unsigned)_buffer.nav_tp5.flags, (unsigned)_buffer.nav_tp5.freqPeriod); +#ifdef HAL_GPIO_PPS + hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING); +#endif const uint16_t desired_flags = 0x003f; const uint16_t desired_period_hz = 1; if (_buffer.nav_tp5.flags != desired_flags || @@ -1531,6 +1534,17 @@ AP_GPS_UBLOX::_parse_gps(void) return false; } +/* + * handle pps interrupt + */ +#ifdef HAL_GPIO_PPS +void +AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us) +{ + _last_pps_time_us = timestamp_us; +} +#endif + // UBlox auto configuration diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index ec4f68d3b8..c23765bfb7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -759,6 +759,10 @@ private: // return true if GPS is capable of F9 config bool supports_F9_config(void) const; +#ifdef HAL_GPIO_PPS + void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us); +#endif + #if GPS_MOVING_BASELINE // config for moving baseline base static const config_list config_MB_Base_uart1[]; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 826b5304f1..9858ce971c 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -249,8 +249,11 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) // get the time the packet arrived on the UART uint64_t uart_us; - if (port) { + if (port && _last_pps_time_us == 0) { uart_us = port->receive_time_constraint_us(msg_length); + } else if (_last_pps_time_us != 0) { + uart_us = _last_pps_time_us; + _last_pps_time_us = 0; } else { uart_us = AP_HAL::micros64(); } @@ -287,6 +290,7 @@ 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; // look for lagged data from the GPS. This is meant to detect @@ -302,6 +306,10 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) state.lagged_sample_count = 0; } } + if (state.status >= AP_GPS::GPS_OK_FIX_2D) { + // we must have a decent fix to calculate difference between itow and pseudo-itow + _pseudo_itow_delta_ms = itow - (_pseudo_itow/1000ULL); + } } } diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 7ebea322f1..e485e6590d 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -91,7 +91,7 @@ public: // return iTOW of last message, or zero if not supported uint32_t get_last_itow(void) const { - return _last_itow; + return (_pseudo_itow_delta_ms == 0)?(_last_itow*1000ULL):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms); } enum DriverOptions : int16_t { @@ -155,10 +155,14 @@ protected: void log_data(const uint8_t *data, uint16_t length); #endif +protected: + uint64_t _last_pps_time_us; + private: // itow from previous message uint32_t _last_itow; uint64_t _pseudo_itow; + int32_t _pseudo_itow_delta_ms; uint32_t _last_ms; uint32_t _rate_ms; uint32_t _last_rate_ms; diff --git a/libraries/AP_RTC/JitterCorrection.h b/libraries/AP_RTC/JitterCorrection.h index 1798d583b2..00d07f43ed 100644 --- a/libraries/AP_RTC/JitterCorrection.h +++ b/libraries/AP_RTC/JitterCorrection.h @@ -16,7 +16,9 @@ public: // correct an offboard timestamp to a jitter-free local // timestamp. See JitterCorrection.cpp for details uint32_t correct_offboard_timestamp_msec(uint32_t offboard_ms, uint32_t local_ms); - + + int64_t get_link_offset_usec(void) const { return link_offset_usec; } + private: const uint16_t max_lag_ms; const uint16_t convergence_loops;