From 9eb6c1be6403abe91243138d3b4a9ebb19dbc308 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 13 Sep 2019 18:50:18 +1000
Subject: [PATCH] AP_GPS: use jitter correction on GPS_INPUT data

this allows for more accurate timing when using GPS_INPUT for indoor
positioning systems
---
 libraries/AP_GPS/AP_GPS_MAV.cpp | 14 ++++++++++++++
 libraries/AP_GPS/AP_GPS_MAV.h   |  2 ++
 2 files changed, 16 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp
index 1b88613c3f..3a26c6e685 100644
--- a/libraries/AP_GPS/AP_GPS_MAV.cpp
+++ b/libraries/AP_GPS/AP_GPS_MAV.cpp
@@ -109,6 +109,20 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 state.have_gps_yaw = true;
             }
 
+            if (packet.fix_type >= 3 && packet.time_week > 0) {
+                /*
+                  use the millisecond timestamp from the GPS_INPUT
+                  packet into jitter correction to get a local
+                  timestamp corrected for transport jitter
+                */
+                if (first_week == 0) {
+                    first_week = packet.time_week;
+                }
+                uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
+                uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, AP_HAL::millis());
+                state.uart_timestamp_ms = corrected_ms;
+            }
+
             state.num_sats = packet.satellites_visible;
             state.last_gps_time_ms = AP_HAL::millis();
             _new_data = true;
diff --git a/libraries/AP_GPS/AP_GPS_MAV.h b/libraries/AP_GPS/AP_GPS_MAV.h
index 8add41e54d..642a47889a 100644
--- a/libraries/AP_GPS/AP_GPS_MAV.h
+++ b/libraries/AP_GPS/AP_GPS_MAV.h
@@ -39,4 +39,6 @@ public:
 
 private:
     bool _new_data;
+    uint32_t first_week;
+    JitterCorrection jitter{2000};
 };