From 3e3151c047587c4bef69289a211a9025a48d745e Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Mon, 11 Nov 2024 13:03:13 +0100 Subject: [PATCH] Avoid septentrio setting wrong time (#23920) --- src/drivers/gnss/septentrio/septentrio.cpp | 47 ++++++++++++---------- src/drivers/gnss/septentrio/septentrio.h | 1 + 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 5c53db6098..9bd5a8bb05 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -1154,34 +1154,36 @@ int SeptentrioDriver::process_message() _message_gps_state.time_utc_usec = 0; #ifndef __PX4_QURT // NOTE: Functionality isn't available on Snapdragon yet. - struct tm timeinfo; - time_t epoch; + if (_time_synced) { + struct tm timeinfo; + time_t epoch; - // Convert to unix timestamp - memset(&timeinfo, 0, sizeof(timeinfo)); + // Convert to unix timestamp + memset(&timeinfo, 0, sizeof(timeinfo)); - timeinfo.tm_year = 1980 - 1900; - timeinfo.tm_mon = 0; - timeinfo.tm_mday = 6 + header.wnc * 7; - timeinfo.tm_hour = 0; - timeinfo.tm_min = 0; - timeinfo.tm_sec = header.tow / 1000; + timeinfo.tm_year = 1980 - 1900; + timeinfo.tm_mon = 0; + timeinfo.tm_mday = 6 + header.wnc * 7; + timeinfo.tm_hour = 0; + timeinfo.tm_min = 0; + timeinfo.tm_sec = header.tow / 1000; - epoch = mktime(&timeinfo); + epoch = mktime(&timeinfo); - if (epoch > k_gps_epoch_secs) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + if (epoch > k_gps_epoch_secs) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - timespec ts; - memset(&ts, 0, sizeof(ts)); - ts.tv_sec = epoch; - ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; - set_clock(ts); + timespec ts; + memset(&ts, 0, sizeof(ts)); + ts.tv_sec = epoch; + ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; + set_clock(ts); - _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; - _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; + _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + } } #endif @@ -1199,6 +1201,7 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { _message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED; + _time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set; } break; diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index ec203cd861..d4b0971bbe 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -712,6 +712,7 @@ private: uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections uint8_t _spoofing_state {0}; ///< Receiver spoofing state uint8_t _jamming_state {0}; ///< Receiver jamming state + bool _time_synced {false}; ///< Receiver time in sync with GPS time const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user static px4::atomic _secondary_instance;