From d3d3c826f72bb5f007a4654eec192681651bc4bd Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 2 Feb 2022 10:01:29 +0530
Subject: [PATCH] AP_GPS: set _last_itow alongside
 state.last_corrected_gps_time_us

---
 libraries/AP_GPS/AP_GPS_MAV.cpp    |  3 +++
 libraries/AP_GPS/AP_GPS_UAVCAN.cpp |  7 ++++++-
 libraries/AP_GPS/GPS_Backend.h     | 11 +++++------
 3 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp
index 57cfd4911b..7581c8ef33 100644
--- a/libraries/AP_GPS/AP_GPS_MAV.cpp
+++ b/libraries/AP_GPS/AP_GPS_MAV.cpp
@@ -123,6 +123,9 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
                 state.last_corrected_gps_time_us = (corrected_ms * 1000ULL);
                 state.corrected_timestamp_updated = true;
+                if (state.last_corrected_gps_time_us) {
+                    _last_itow = state.time_week_ms;
+                }
                 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 4f0310d171..ea57ca9ccd 100644
--- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
@@ -876,7 +876,12 @@ bool AP_GPS_UAVCAN::read(void)
         interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0);
 
         state = interim_state;
-
+        if (interim_state.last_corrected_gps_time_us) {
+            // If we were able to get a valid last_corrected_gps_time_us
+            // we have had a valid GPS message time, from which we calculate
+            // the time of week.
+            _last_itow = interim_state.time_week_ms;
+        }
         return true;
     }
     if (!seen_message) {
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index 5c0213bd37..d0d5cda68f 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -95,7 +95,7 @@ public:
 
     // return iTOW of last message, or zero if not supported
     uint32_t get_last_itow(void) const {
-        return (_pseudo_itow_delta_ms == 0)?(_last_itow*1000ULL):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
+        return (_pseudo_itow_delta_ms == 0)?(_last_itow):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
     }
 
     enum DriverOptions : int16_t {
@@ -110,6 +110,10 @@ protected:
     AP_GPS &gps;                        ///< access to frontend (for parameters)
     AP_GPS::GPS_State &state;           ///< public state for this instance
 
+    uint64_t _last_pps_time_us;
+    JitterCorrection jitter_correction;
+    uint32_t _last_itow;
+
     // common utility functions
     int32_t swap_int32(int32_t v) const;
     int16_t swap_int16(int16_t v) const;
@@ -161,13 +165,8 @@ protected:
     void log_data(const uint8_t *data, uint16_t length);
 #endif
 
-protected:
-    uint64_t _last_pps_time_us;
-    JitterCorrection jitter_correction;
-
 private:
     // itow from previous message
-    uint32_t _last_itow;
     uint64_t _pseudo_itow;
     int32_t _pseudo_itow_delta_ms;
     uint32_t _last_ms;