mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: ensure we have full timestamp before setting RTC clock
thanks to Martin Sollie for this bug report
This commit is contained in:
parent
ef9c65c136
commit
d515b40966
|
@ -356,7 +356,7 @@ uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms)
|
|||
uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const
|
||||
{
|
||||
const GPS_State &istate = state[instance];
|
||||
if (istate.last_gps_time_ms == 0) {
|
||||
if (istate.last_gps_time_ms == 0 || istate.time_week == 0) {
|
||||
return 0;
|
||||
}
|
||||
uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms);
|
||||
|
@ -674,9 +674,11 @@ void AP_GPS::update_instance(uint8_t instance)
|
|||
|
||||
if (state[instance].status >= GPS_OK_FIX_3D) {
|
||||
const uint64_t now = time_epoch_usec(instance);
|
||||
if (now != 0) {
|
||||
AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update all GPS instances
|
||||
|
|
Loading…
Reference in New Issue