From 6c9cbe1a58a12c536196b817bec3b05d9d119bd8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 24 Nov 2022 08:44:47 +1100
Subject: [PATCH] AP_GPS: fixed NMEA time reporting

basic NMEA does not have an itow, so we need to use time_week_ms

this caused us to always display the time as the start of the GPS week
on NMEA
---
 libraries/AP_GPS/AP_GPS_MAV.cpp    |  1 +
 libraries/AP_GPS/AP_GPS_UAVCAN.cpp |  1 +
 libraries/AP_GPS/GPS_Backend.cpp   | 12 ++++++++++++
 libraries/AP_GPS/GPS_Backend.h     |  5 ++---
 4 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp
index eab22b3e50..41341bd52d 100644
--- a/libraries/AP_GPS/AP_GPS_MAV.cpp
+++ b/libraries/AP_GPS/AP_GPS_MAV.cpp
@@ -125,6 +125,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 state.corrected_timestamp_updated = true;
                 if (state.last_corrected_gps_time_us) {
                     _last_itow_ms = state.time_week_ms;
+                    _have_itow = true;
                 }
                 if (have_yaw) {
                     state.gps_yaw_time_ms = corrected_ms;
diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
index d125d494da..ea0c5bea0a 100644
--- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
@@ -767,6 +767,7 @@ bool AP_GPS_UAVCAN::read(void)
             // we have had a valid GPS message time, from which we calculate
             // the time of week.
             _last_itow_ms = interim_state.time_week_ms;
+            _have_itow = true;
         }
         return true;
     }
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index 7fdcb28914..dc45830840 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/libraries/AP_GPS/GPS_Backend.cpp
@@ -109,6 +109,17 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
     state.time_week_ms += msec;
 }
 
+/*
+  get the last time of week in ms
+ */
+uint32_t AP_GPS_Backend::get_last_itow_ms(void) const
+{
+    if (!_have_itow) {
+        return state.time_week_ms;
+    }
+    return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
+}
+
 /*
   fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
  */
@@ -237,6 +248,7 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 {
     if (itow != _last_itow_ms) {
         _last_itow_ms = itow;
+        _have_itow = true;
 
         /*
           we need to calculate a pseudo-itow, which copes with the
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index f4bb25bd25..019f0c76b3 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -91,9 +91,7 @@ public:
     virtual bool get_error_codes(uint32_t &error_codes) const { return false; }
 
     // return iTOW of last message, or zero if not supported
-    uint32_t get_last_itow_ms(void) const {
-        return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
-    }
+    uint32_t get_last_itow_ms(void) const;
 
     // check if an option is set
     bool option_set(const AP_GPS::DriverOptions option) const {
@@ -108,6 +106,7 @@ protected:
     uint64_t _last_pps_time_us;
     JitterCorrection jitter_correction;
     uint32_t _last_itow_ms;
+    bool _have_itow;
 
     // common utility functions
     int32_t swap_int32(int32_t v) const;