diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index eb99f406ec..b56b7cc4fa 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -284,7 +284,7 @@ AP_GPS_SBF::process_message(void) state.time_week_ms = (uint32_t)(temp.TOW); } - set_uart_timestamp(sbf_msg.length); + check_new_itow(temp.TOW, sbf_msg.length); state.last_gps_time_ms = AP_HAL::millis(); // Update velocity state (don't use −2·10^10) @@ -361,6 +361,7 @@ AP_GPS_SBF::process_message(void) case DOP: { const msg4001 &temp = sbf_msg.data.msg4001u; + check_new_itow(temp.TOW, sbf_msg.length); state.hdop = temp.HDOP; state.vdop = temp.VDOP; @@ -369,6 +370,7 @@ AP_GPS_SBF::process_message(void) case ReceiverStatus: { const msg4014 &temp = sbf_msg.data.msg4014u; + check_new_itow(temp.TOW, sbf_msg.length); RxState = temp.RxState; if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) { gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF error changed (0x%08x/0x%08x)", state.instance + 1, @@ -381,6 +383,7 @@ AP_GPS_SBF::process_message(void) { const msg5908 &temp = sbf_msg.data.msg5908u; + check_new_itow(temp.TOW, sbf_msg.length); // select the maximum variance, as the EKF will apply it to all the columns in it's estimate // FIXME: Support returning the covariance matrix to the EKF float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu)); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index f67a5a1155..018c465a8f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1336,10 +1336,8 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const } } +// uBlox specific check_new_itow(), handling message length 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); - } + check_new_itow(itow, _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 293b073ff9..e48674e5c2 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -549,9 +549,6 @@ 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); diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index b903e7858a..2c834c93a8 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -227,3 +227,12 @@ void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes) state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U; } } + + +void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) +{ + if (itow != _last_itow) { + _last_itow = itow; + set_uart_timestamp(msg_length); + } +} diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 927f86e0ad..3fa8de76ea 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -93,4 +93,10 @@ protected: assuming the message started nbytes ago */ void set_uart_timestamp(uint16_t nbytes); + + void check_new_itow(uint32_t itow, uint32_t msg_length); + +private: + // itow from previous message + uint32_t _last_itow; };