2 files changed, 19 insertions, 1 deletions
diff --git a/lib/edi/common.cpp b/lib/edi/common.cpp
index d6ca89c..8d99619 100644
--- a/lib/edi/common.cpp
+++ b/lib/edi/common.cpp
@@ -73,6 +73,19 @@ double frame_timestamp_t::diff_ms(const frame_timestamp_t& other) const
     return lhs - rhs;
 }
 
+frame_timestamp_t& frame_timestamp_t::operator+=(const std::chrono::milliseconds& ms)
+{
+    tsta += (ms.count() % 1000) << 14; // Shift ms by 14 to Timestamp level 2
+    if (tsta > 0xf9FFff) {
+        tsta -= 0xfa0000; // Substract 16384000, corresponding to one second
+        seconds += 1;
+    }
+
+    seconds += (ms.count() / 1000);
+
+    return *this;
+}
+
 frame_timestamp_t frame_timestamp_t::from_unix_epoch(std::time_t time, uint32_t tai_utc_offset, uint32_t tsta)
 {
     frame_timestamp_t ts;
@@ -90,7 +103,10 @@ 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));
+    // We cannot use nanosecond resolution because not all platforms use a
+    // system_clock that has nanosecond precision. It's not really important,
+    // as this function is only used for debugging.
+    ts += chrono::microseconds(std::lrint(tsta / 16.384));
 
     return ts;
 }
diff --git a/lib/edi/common.hpp b/lib/edi/common.hpp
index 8252a7a..2a9c683 100644
--- a/lib/edi/common.hpp
+++ b/lib/edi/common.hpp
@@ -43,6 +43,8 @@ struct frame_timestamp_t {
 
     double diff_ms(const frame_timestamp_t& other) const;
 
+    frame_timestamp_t& operator+=(const std::chrono::milliseconds& ms);
+
     static frame_timestamp_t from_unix_epoch(std::time_t time, uint32_t tai_utc_offset, uint32_t tsta);
 };