diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index fd77f56dc3..d6762d82e9 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -28,6 +28,8 @@ #include #include "RTCM3_Parser.h" #include +#include +#include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH @@ -1580,6 +1582,7 @@ AP_GPS_UBLOX::_parse_gps(void) _check_new_itow(_buffer.solution.itow); if (havePvtMsg) { state.time_week = _buffer.solution.week; + _have_receiver_week = true; break; } if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) { @@ -1606,6 +1609,7 @@ AP_GPS_UBLOX::_parse_gps(void) state.last_gps_time_ms = AP_HAL::millis(); state.time_week_ms = _buffer.solution.itow; state.time_week = _buffer.solution.week; + _have_receiver_week = true; } #if UBLOX_FAKE_3DLOCK next_fix = state.status; @@ -1744,6 +1748,39 @@ AP_GPS_UBLOX::_parse_gps(void) // time state.time_week_ms = _buffer.pvt.itow; + + // RoboFusion patch: some UBX-compatible receivers (for example + // Unicore UM626N based modules) only output NAV-PVT and never send + // NAV-SOL or NAV-TIMEGPS, so time_week stays 0 and the vehicle never + // gets a valid GPS time. Until the receiver supplies the week itself, + // derive it from the PVT UTC date/time. The receiver's own iTOW is + // used as the reference and the result is rounded to the nearest + // week, so a leap-second or rounding offset cannot shift the week. + if (!_have_receiver_week && + (_buffer.pvt.valid & 0x03) == 0x03 && // validDate and validTime + _buffer.pvt.year >= 2020 && _buffer.pvt.year < 2100 && + _buffer.pvt.month >= 1 && _buffer.pvt.month <= 12 && + _buffer.pvt.day >= 1 && _buffer.pvt.day <= 31) { + struct tm tm {}; + tm.tm_year = _buffer.pvt.year - 1900; + tm.tm_mon = _buffer.pvt.month - 1; + tm.tm_mday = _buffer.pvt.day; + tm.tm_hour = _buffer.pvt.hour; + tm.tm_min = _buffer.pvt.min; + tm.tm_sec = _buffer.pvt.sec; + const uint32_t unix_secs = uint32_t(ap_mktime(&tm)); + const uint32_t unix_to_gps_secs = 315964800UL; + const uint32_t leap_secs = uint32_t(GPS_LEAPSECONDS_MILLIS / 1000U); + const uint32_t gps_secs = unix_secs + leap_secs - unix_to_gps_secs; + const uint32_t itow_secs = _buffer.pvt.itow / 1000U; + const uint32_t week = (gps_secs - itow_secs + AP_SEC_PER_WEEK / 2U) / AP_SEC_PER_WEEK; + state.time_week = uint16_t(week); + if (!_pvt_week_announced) { + _pvt_week_announced = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox %d: GPS week %u from NAV-PVT date", + state.instance + 1, unsigned(week)); + } + } #if UBLOX_FAKE_3DLOCK state.location.lng = 1491652300L; state.location.lat = -353632610L; @@ -1765,6 +1802,7 @@ AP_GPS_UBLOX::_parse_gps(void) _check_new_itow(_buffer.timegps.itow); if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) { state.time_week = _buffer.timegps.week; + _have_receiver_week = true; } break; case MSG_VELNED: diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 47d1ccbd3f..b83c5c398e 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -812,6 +812,12 @@ private: bool havePvtMsg; + // RoboFusion: true once the receiver itself has supplied the GPS week + // (NAV-SOL or NAV-TIMEGPS). Until then the week is derived from the + // NAV-PVT UTC date, for PVT-only receivers such as UM626N modules. + bool _have_receiver_week { false }; + bool _pvt_week_announced { false }; + // structure for list of config key/value pairs for // specific configurations struct PACKED config_list {