diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 4954b46ff6..611be68487 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -235,6 +235,29 @@ const uint32_t AP_GPS::_baudrates[] = {4800U, 19200U, 38400U, 115200U, 57600U, 9 // right mode const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; +/** + convert GPS week and milliseconds to unix epoch in milliseconds + */ +uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) +{ + uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * MSEC_PER_WEEK + gps_ms; + return fix_time_ms; +} + +/** + calculate current time since the unix epoch in microseconds + */ +uint64_t AP_GPS::time_epoch_usec(uint8_t instance) +{ + const GPS_State &istate = state[instance]; + if (istate.last_gps_time_ms == 0) { + return 0; + } + uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms); + // add in the milliseconds since the last fix + return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; +} + /* send some more initialisation string bytes if there is room in the UART transmit buffer diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 3b9e1b3304..f4bbc515bb 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -58,29 +58,6 @@ int16_t AP_GPS_Backend::swap_int16(int16_t v) const return u.v; } -/** - convert GPS week and milliseconds to unix epoch in milliseconds - */ -uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) -{ - uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * MSEC_PER_WEEK + gps_ms; - return fix_time_ms; -} - -/** - calculate current time since the unix epoch in microseconds - */ -uint64_t AP_GPS::time_epoch_usec(uint8_t instance) -{ - const GPS_State &istate = state[instance]; - if (istate.last_gps_time_ms == 0) { - return 0; - } - uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms); - // add in the milliseconds since the last fix - return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; -} - /** fill in time_week_ms and time_week from BCD date and time components