From 5da80f44b18b990214f806a2897da0d9d8fda16d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Mar 2015 10:29:36 +1100 Subject: [PATCH] AP_GPS: fixed MTK1.6 time handling my MTK1.6 does not use hectoseconds, it uses milliseconds --- libraries/AP_GPS/AP_GPS_MTK19.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index c937cab4f0..5968082bcb 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -151,11 +151,13 @@ restart: if (state.status >= AP_GPS::GPS_OK_FIX_2D) { if (_fix_counter == 0) { uint32_t bcd_time_ms; - if (_mtk_revision == MTK_GPS_REVISION_V16) { - bcd_time_ms = _buffer.msg.utc_time*10; - } else { - bcd_time_ms = _buffer.msg.utc_time; - } + bcd_time_ms = _buffer.msg.utc_time; +#if 0 + hal.console->printf("utc_date=%lu utc_time=%lu rev=%u\n", + (unsigned long)_buffer.msg.utc_date, + (unsigned long)_buffer.msg.utc_time, + (unsigned)_mtk_revision); +#endif make_gps_time(_buffer.msg.utc_date, bcd_time_ms); state.last_gps_time_ms = hal.scheduler->millis(); }