diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 432ce26376bb5..b2a858a16f303 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -556,6 +556,7 @@ struct PACKED log_Performance { uint32_t i2c_count; uint32_t i2c_isr_count; uint32_t extra_loop_us; + uint64_t rtc; }; struct PACKED log_SRTL { @@ -895,14 +896,15 @@ struct PACKED log_VER { // @Field: MaxT: Maximum loop time // @Field: Mem: Free memory available // @Field: Load: System processor load -// @Field: IntE: Internal error mask; which internal errors have been detected +// @Field: InE: Internal error mask; which internal errors have been detected // @FieldBitmaskEnum: IntE: AP_InternalError::error_t // @Field: ErrL: Internal error line number; last line number on which a internal error was detected -// @Field: ErrC: Internal error count; how many internal errors have been detected +// @Field: ErC: Internal error count; how many internal errors have been detected // @Field: SPIC: Number of SPI transactions processed // @Field: I2CC: Number of i2c transactions processed // @Field: I2CI: Number of i2c interrupts serviced // @Field: Ex: number of microseconds being added to each loop to address scheduler overruns +// @Field: R: RTC time, time since Unix epoch // @LoggerMessage: POWR // @Description: System power information @@ -1211,7 +1213,7 @@ LOG_STRUCTURE_FROM_MOUNT \ LOG_STRUCTURE_FROM_BEACON \ LOG_STRUCTURE_FROM_PROXIMITY \ { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ - "PM", "QHHHIIHHIIIIII", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "sz---b%------s", "F----0A------F" }, \ + "PM", "QHHHIIHHIIIIIIQ", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,InE,ErC,SPIC,I2CC,I2CI,Ex,R", "sz---b%------ss", "F----0A------FF" }, \ { LOG_SRTL_MSG, sizeof(log_SRTL), \ "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \ LOG_STRUCTURE_FROM_AVOIDANCE \ diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index a57c3242852fd..7a2f6e7efd44b 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 diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 7085e05541d22..0c3ae78f87e2b 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -447,6 +447,11 @@ void AP_Scheduler::update_logging() // Write a performance monitoring packet void AP_Scheduler::Log_Write_Performance() { + uint64_t rtc = 0; +#if AP_RTC_ENABLED + UNUSED_RESULT(AP::rtc().get_utc_usec(rtc)); +#endif + const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), @@ -464,6 +469,7 @@ void AP_Scheduler::Log_Write_Performance() i2c_count : pd.i2c_count, i2c_isr_count : pd.i2c_isr_count, extra_loop_us : extra_loop_us, + rtc : rtc, }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); }