mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_GPS: don't accept a zero GNSS timestamp from UAVCAN GPS
this is needed for a bug in AP_Periph 1.0 which could briefly send a zero timestamp on first fix marked as a UTC time
This commit is contained in:
parent
b0e7251832
commit
00d99c5177
@ -173,12 +173,14 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
|
||||
|
||||
if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) {
|
||||
uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec();
|
||||
if (epoch_ms != 0) {
|
||||
epoch_ms /= 1000;
|
||||
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
|
||||
interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
|
||||
interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (process) {
|
||||
Location loc = { };
|
||||
@ -283,11 +285,13 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
|
||||
|
||||
if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC) {
|
||||
uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec();
|
||||
if (epoch_ms != 0) {
|
||||
epoch_ms /= 1000;
|
||||
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
|
||||
interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
|
||||
interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK);
|
||||
}
|
||||
}
|
||||
|
||||
if (interim_state.status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
|
||||
if (cb.msg->mode == uavcan::equipment::gnss::Fix2::MODE_DGPS) {
|
||||
|
Loading…
Reference in New Issue
Block a user