2 files changed, 29 insertions, 2 deletions
diff --git a/lib/edi/common.cpp b/lib/edi/common.cpp
index d9335af..b4b0c79 100644
--- a/lib/edi/common.cpp
+++ b/lib/edi/common.cpp
@@ -25,18 +25,31 @@
 #include <iomanip>
 #include <sstream>
 #include <cassert>
+#include <cmath>
 #include <cstdio>
 
 namespace EdiDecoder {
 
 using namespace std;
 
+bool frame_timestamp_t::valid() const
+{
+    return tsta != 0xFFFFFF;
+}
+
 string frame_timestamp_t::to_string() const
 {
     const time_t seconds_in_unix_epoch = to_unix_epoch();
 
     stringstream ss;
-    ss << "Timestamp: " << std::put_time(std::gmtime(&seconds_in_unix_epoch), "%c %Z");
+    if (valid()) {
+        ss << "Timestamp: ";
+    }
+    else {
+        ss << "Timestamp not valid: ";
+    }
+    ss << std::put_time(std::gmtime(&seconds_in_unix_epoch), "%c %Z") <<
+        " + " << ((double)tsta / 16384000.0);
     return ss.str();
 }
 
@@ -48,6 +61,16 @@ time_t frame_timestamp_t::to_unix_epoch() const
     return 946684800 + seconds - utco;
 }
 
+std::chrono::system_clock::time_point frame_timestamp_t::to_system_clock() const
+{
+    auto ts = chrono::system_clock::from_time_t(to_unix_epoch());
+
+    // PPS offset in seconds = tsta / 16384000
+    ts += chrono::nanoseconds(std::lrint(tsta / 0.016384));
+
+    return ts;
+}
+
 
 TagDispatcher::TagDispatcher(
         std::function<void()>&& af_packet_completed, bool verbose) :
@@ -179,7 +202,7 @@ decode_state_t TagDispatcher::decode_afpacket(
         return {false, 0};
     }
 
-    if (m_last_seq + (uint16_t)1 != seq) {
+    if (m_last_seq + 1 != seq) {
         etiLog.level(warn) << "EDI AF Packet sequence error, " << seq;
     }
     m_last_seq = seq;
diff --git a/lib/edi/common.hpp b/lib/edi/common.hpp
index 1433004..887bc3d 100644
--- a/lib/edi/common.hpp
+++ b/lib/edi/common.hpp
@@ -23,6 +23,7 @@
 #include "PFT.hpp"
 #include <functional>
 #include <map>
+#include <chrono>
 #include <string>
 #include <vector>
 #include <cstddef>
@@ -33,9 +34,12 @@ namespace EdiDecoder {
 struct frame_timestamp_t {
     uint32_t seconds = 0;
     uint32_t utco = 0;
+    uint32_t tsta = 0; // According to EN 300 797 Annex B
 
+    bool valid() const;
     std::string to_string() const;
     time_t to_unix_epoch() const;
+    std::chrono::system_clock::time_point to_system_clock() const;
 };
 
 struct decode_state_t {