From b02c414cade436f6b06376077a216e4720f339cf Mon Sep 17 00:00:00 2001
From: Oliver Jowett <oliver@mutability.co.uk>
Date: Mon, 24 Jun 2019 13:37:50 -0500
Subject: [PATCH] More tweaking of stratux timestamp handling

---
 stratux_serial.cc | 27 ++++++++++++++-------------
 stratux_serial.h  |  2 +-
 2 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/stratux_serial.cc b/stratux_serial.cc
index cbdc2b8..92e4755 100644
--- a/stratux_serial.cc
+++ b/stratux_serial.cc
@@ -99,6 +99,8 @@ void StratuxSerial::ParseInput(const Bytes &buf) {
     // 2000000Mbps, 8N1 = 200,000 bytes/s = 200 bytes/ms
     static auto unix_epoch = std::chrono::system_clock::from_time_t(0);
     auto start_of_read = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - unix_epoch).count() - buf.size() / 200;
+    std::uint64_t previous_sys_timestamp = 0;
+    std::uint32_t previous_raw_timestamp = 0;
 
     for (auto i = buf.begin(); i != buf.end();) {
         switch (parser_state_) {
@@ -138,9 +140,17 @@ void StratuxSerial::ParseInput(const Bytes &buf) {
             i += bytes_to_copy;
 
             if (message_.size() == message_length_) {
-                auto previous_sys_timestamp = (messages ? messages->front().ReceivedAt() : message_start_timestamp_);
-                auto previous_raw_timestamp = (messages ? messages->front().RawTimestamp() : 0);
-                auto parsed = ParseMessage(message_, previous_sys_timestamp, previous_raw_timestamp);
+                // work out a suitable timestamp
+                std::uint32_t raw_timestamp = message_[1] | (message_[2] << 8) | (message_[3] << 16) | (message_[4] << 24);
+                std::uint64_t sys_timestamp;
+                if (previous_sys_timestamp != 0 && raw_timestamp > previous_raw_timestamp) {
+                    sys_timestamp = previous_sys_timestamp + (raw_timestamp - previous_raw_timestamp) / 4000;
+                } else {
+                    sys_timestamp = previous_sys_timestamp = message_start_timestamp_;
+                    previous_raw_timestamp = raw_timestamp;
+                }
+
+                auto parsed = ParseMessage(message_, sys_timestamp);
                 if (parsed) {
                     if (!messages) {
                         messages = std::make_shared<MessageVector>();
@@ -164,7 +174,7 @@ void StratuxSerial::ParseInput(const Bytes &buf) {
     }
 }
 
-boost::optional<RawMessage> StratuxSerial::ParseMessage(const Bytes &message, std::uint64_t previous_sys_timestamp, std::uint32_t previous_raw_timestamp) {
+boost::optional<RawMessage> StratuxSerial::ParseMessage(const Bytes &message, std::uint64_t sys_timestamp) {
     assert(message.size() >= 5);
 
     // not entirely clear what the RSSI format is; here we assume it's
@@ -174,15 +184,6 @@ boost::optional<RawMessage> StratuxSerial::ParseMessage(const Bytes &message, st
 
     std::uint32_t raw_timestamp = message[1] | (message[2] << 8) | (message[3] << 16) | (message[4] << 24);
 
-    // If this is the first message seen in a read, use the system timestamp directly.
-    // Otherwise, use the reported timestamp relative to the first message
-    std::uint64_t sys_timestamp;
-    if (previous_raw_timestamp == 0 || raw_timestamp < previous_raw_timestamp) {
-        sys_timestamp = previous_sys_timestamp;
-    } else {
-        sys_timestamp = previous_sys_timestamp + (raw_timestamp - previous_raw_timestamp) / 4000;
-    }
-
     Bytes payload;
     std::copy(message.begin() + 5, message.end(), std::back_inserter(payload));
 
diff --git a/stratux_serial.h b/stratux_serial.h
index 21d6775..2d290bd 100644
--- a/stratux_serial.h
+++ b/stratux_serial.h
@@ -40,7 +40,7 @@ namespace flightaware::uat {
       private:
         void StartReading();
         void ParseInput(const Bytes &buf);
-        boost::optional<RawMessage> ParseMessage(const Bytes &message, std::uint64_t previous_sys_timestamp, std::uint32_t previous_raw_timestamp);
+        boost::optional<RawMessage> ParseMessage(const Bytes &message, std::uint64_t sys_timestamp);
         void HandleError(const boost::system::error_code &ec);
 
         // the size of the read buffer