Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: don't accept a zero GNSS timestamp from UAVCAN GPS

Browse Source 
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:
Andrew Tridgell 2020-04-29 09:36:16 +10:00
parent b0e7251832
commit 00d99c5177
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -173,10 +173,12 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) { 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(); uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec();
epoch_ms /= 1000; if (epoch_ms != 0) {
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC; epoch_ms /= 1000;
interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK); uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK); 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);
}
} }
} }
@ -283,10 +285,12 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC) { 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(); uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec();
epoch_ms /= 1000; if (epoch_ms != 0) {
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC; epoch_ms /= 1000;
interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK); uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK); 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 (interim_state.status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) {