diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 95f7599b0d..5a545ca553 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -523,6 +523,19 @@ uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const if ((istate.last_gps_time_ms == 0 && istate.last_corrected_gps_time_us == 0) || istate.time_week == 0) { return 0; } + //////////////////// + static uint32_t last_1s = AP_HAL::millis(); + if (AP_HAL::millis() - last_1s > 5000) + { + last_1s = AP_HAL::millis(); + uint64_t temp1_time_ms,temp2_time_ms; + temp1_time_ms = istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms()); + temp2_time_ms = istate_time_to_epoch_ms(istate.time_week, istate.time_week_ms); + gcs().send_text(MAV_SEVERITY_INFO, "epoch:%lld,utc:%lld,now:%lld",istate.last_corrected_gps_time_us ,(temp1_time_ms*1000ULL) + (AP_HAL::micros64() - istate.last_corrected_gps_time_us),(temp2_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL); + gcs().send_text(MAV_SEVERITY_INFO, "epoch ms:%ld, %ld", drivers[instance]->get_last_itow_ms(), istate.time_week_ms ); + + } + //////////////////// uint64_t fix_time_ms; // add in the time since the last fix message if (istate.last_corrected_gps_time_us != 0) { diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 84a487642c..c9cce75d4f 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -107,15 +107,15 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) state.time_week = ret / AP_SEC_PER_WEEK; state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; state.time_week_ms += msec; - + check_new_itow(state.time_week_ms, 10); static uint32_t last_1s = AP_HAL::millis(); if (AP_HAL::millis() - last_1s > 5000) { // gcs().send_text(MAV_SEVERITY_INFO, "week:%d,ms:%ld",state.time_week,state.time_week_ms); // gcs().send_text(MAV_SEVERITY_INFO, "year:%d,m:%d,d:%d \n\r",tm.tm_year,tm.tm_mon,tm.tm_mday); // gcs().send_text(MAV_SEVERITY_INFO, "hour:%d,m:%d,s:%d \n\r",tm.tm_hour,tm.tm_min,tm.tm_sec); - gcs().send_text(MAV_SEVERITY_INFO, "unix_time:%ld",unix_time); - gcs().send_text(MAV_SEVERITY_INFO,"now: %s\n",AP::rtc().get_rtc_date()); + gcs().send_text(MAV_SEVERITY_INFO, "unix_time:%ld,%04d-%02d-%02d %02d:%02d:%02d",unix_time,tm.tm_year+ 1900 - 1,tm.tm_mon+1,tm.tm_mday,tm.tm_hour,tm.tm_min,tm.tm_sec); + // gcs().send_text(MAV_SEVERITY_INFO,"now: %s\n",AP::rtc().get_rtc_date()); last_1s = AP_HAL::millis(); } } diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index 319dc7060b..7beca31b65 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -52,30 +52,24 @@ 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 if (need_prt) { - gcs().send_text(MAV_SEVERITY_INFO, "type{%d} >= rtc_source_type{%d}",type , (uint8_t)rtc_source_type); + get_rtc_date(); + gcs().send_text(MAV_SEVERITY_INFO, "type:%d,utc:%lld,now:%lld",type ,time_utc_usec,AP_HAL::micros64()); } + if (type >= rtc_source_type && !need_prt) { + // e.g. system-time message when we've been set by the GPS + return; } // check it's from an allowed sources: if (!(allowed_types & (1< 5) { - gcs().send_text(MAV_SEVERITY_INFO, "tmp{%lld} < rtc_shift{%lld}",tmp , rtc_shift); + sh_cnt = 0; + goto Forc_Shift; } + + return; } +Forc_Shift: WITH_SEMAPHORE(rsem); rtc_shift = tmp; @@ -100,10 +102,9 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type) rtc_source_type = type; - if (need_prt) - { + gcs().send_text(MAV_SEVERITY_INFO, "type:%d,in utc:%lld,rtc_shift:%lld",type,time_utc_usec,rtc_shift); - } + #if HAL_GCS_ENABLED // update signing timestamp GCS_MAVLINK::update_signing_timestamp(time_utc_usec); @@ -251,7 +252,9 @@ char* AP_RTC::get_rtc_date(void) min = uint8_t(localtime_tm.tm_min); sec = uint8_t(localtime_tm.tm_sec); static char buf[50]; - snprintf(buf, sizeof(buf), "%04d%02d%02d%02d%02d%02d",year,mon,mday,hour,min,sec); + snprintf(buf, sizeof(buf), "%04d-%02d-%02d %02d:%02d:%02d",year,mon,mday,hour,min,sec); + gcs().send_text(MAV_SEVERITY_INFO, "get_rtc: %lld,%s",time_usec,buf); + return buf; }