diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index a57c324285..7a2f6e7efd 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -9,6 +9,12 @@ #include #include +#define DEBUG_RTC_SHIFT 0 + +#if DEBUG_RTC_SHIFT +#include +#endif + extern const AP_HAL::HAL& hal; AP_RTC::AP_RTC() @@ -47,9 +53,19 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type) { const uint64_t oldest_acceptable_date_us = 1640995200ULL*1000*1000; // 2022-01-01 0:00 - if (type >= rtc_source_type) { - // e.g. system-time message when we've been set by the GPS - return; + // only allow time to be moved forward from the same sourcetype + // while the vehicle is disarmed: + if (hal.util->get_soft_armed()) { + if (type >= rtc_source_type) { + // e.g. system-time message when we've been set by the GPS + return; + } + } else { + // vehicle is disarmed; accept (e.g.) GPS time source updates + if (type > rtc_source_type) { + // e.g. system-time message when we've been set by the GPS + return; + } } // check it's from an allowed sources: @@ -70,6 +86,11 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type) } WITH_SEMAPHORE(rsem); +#if DEBUG_RTC_SHIFT + uint64_t old_utc = 0; + UNUSED_RESULT(get_utc_usec(old_utc)); +#endif + rtc_shift = tmp; // update hardware clock: @@ -83,6 +104,32 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type) // update signing timestamp GCS_MAVLINK::update_signing_timestamp(time_utc_usec); #endif + +#if DEBUG_RTC_SHIFT + uint64_t new_utc = 0; + UNUSED_RESULT(get_utc_usec(new_utc)); + if (old_utc != new_utc) { + if (AP::logger().should_log(0xFFFF)){ + // log to AP_Logger + // @LoggerMessage: RTC + // @Description: Information about RTC clock resets + // @Field: TimeUS: Time since system startup + // @Field: old: old time + // @Field: new: new time + // @Field: in: new argument time + AP::logger().WriteStreaming( + "RTC", + "TimeUS,old_utc,new_utc", + "sss", + "FFF", + "QQQ", + AP_HAL::micros64(), + old_utc, + new_utc + ); + } + } +#endif } bool AP_RTC::get_utc_usec(uint64_t &usec) const