From cccd556ca20560168e3827c7dd09e373750127cb Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 5 May 2025 19:13:18 -0800 Subject: [PATCH 1/2] mavlink: set system clock from SYSTEM_TIME message if behind by more than 60 seconds --- src/modules/mavlink/mavlink_timesync.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index 410dd84512a6..018a57198f2e 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -86,11 +86,14 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) timespec tv = {}; px4_clock_gettime(CLOCK_REALTIME, &tv); - // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; - bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000000ULL; - - if (!onb_unix_valid && ofb_unix_valid) { + // Set the system time if we are lagging behind by more than a minute + hrt_abstime seconds_behind = 60; + hrt_abstime local_time = tv.tv_sec; + hrt_abstime remote_time = time.time_unix_usec / 1000000; + bool update = (remote_time > PX4_EPOCH_SECS) && (remote_time > local_time + seconds_behind); + + if (update) { + PX4_INFO("Setting system clock"); tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; From 3bab5e00af0cd8e5c3a93cde920b95d364fcff08 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 11 May 2025 20:12:45 -0800 Subject: [PATCH 2/2] change hrt_abstime to time_t (uint64_t to uint32_t). Updated INFO message --- src/modules/mavlink/mavlink_timesync.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index 018a57198f2e..c9df0a412631 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -87,13 +87,13 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) px4_clock_gettime(CLOCK_REALTIME, &tv); // Set the system time if we are lagging behind by more than a minute - hrt_abstime seconds_behind = 60; - hrt_abstime local_time = tv.tv_sec; - hrt_abstime remote_time = time.time_unix_usec / 1000000; + time_t seconds_behind = 60; + time_t local_time = tv.tv_sec; + time_t remote_time = time.time_unix_usec / 1000000; bool update = (remote_time > PX4_EPOCH_SECS) && (remote_time > local_time + seconds_behind); if (update) { - PX4_INFO("Setting system clock"); + PX4_INFO("Setting system clock from SYSTEM_TIME sent by %d/%d", msg->sysid, msg->compid); tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;