Improve timestamp handling

Replace PPS representation from double to integer, and analyse
in OutputUHD if the timestamp is as expected.

6 files changed, 124 insertions, 114 deletions

diff --git a/src/EtiReader.cpp b/src/EtiReader.cpp
index 16272e3..067d6d0 100644
--- a/src/EtiReader.cpp
+++ b/src/EtiReader.cpp
@@ -287,7 +287,7 @@ bool EtiReader::sourceContainsTimestamp()
     /* See ETS 300 799, Annex C.2.2 */
 }
 
-double EtiReader::getPPSOffset()
+uint32_t EtiReader::getPPSOffset()
 {
     if (!sourceContainsTimestamp()) {
         //fprintf(stderr, "****** SOURCE NO TS\n");
@@ -296,8 +296,7 @@ double EtiReader::getPPSOffset()
 
     uint32_t timestamp = ntohl(eti_tist.TIST) & 0xFFFFFF;
     //fprintf(stderr, "****** TIST 0x%x\n", timestamp);
-    double pps = timestamp / 16384000.0; // seconds
 
-    return pps;
+    return timestamp;
 }
 
diff --git a/src/EtiReader.h b/src/EtiReader.h
index c9cbe07..4c0d4f3 100644
--- a/src/EtiReader.h
+++ b/src/EtiReader.h
@@ -67,8 +67,8 @@ public:
     bool sourceContainsTimestamp();
 
 protected:
-    /* Transform the ETI TIST to a PPS offset in ms */
-    double getPPSOffset();
+    /* Transform the ETI TIST to a PPS offset in units of 1/16384000 s */
+    uint32_t getPPSOffset();
 
     void sync();
     int state;
diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp
index b9f800e..2325197 100644
--- a/src/OutputUHD.cpp
+++ b/src/OutputUHD.cpp
@@ -328,17 +328,16 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
         if (first_run) {
             etiLog.level(debug) << "OutputUHD: UHD initialising...";
 
-            uwd.sourceContainsTimestamp = myConf.enableSync &&
-                myEtiReader->sourceContainsTimestamp();
-
             switch (myEtiReader->getMode()) {
-                case 1: uwd.fct_increment = 4; break;
+                case 1: fct_increment = 4; break;
                 case 2:
-                case 3: uwd.fct_increment = 1; break;
-                case 4: uwd.fct_increment = 2; break;
+                case 3: fct_increment = 1; break;
+                case 4: fct_increment = 2; break;
                 default: break;
             }
 
+            last_fct = -1;
+
             // we only set the delay buffer from the dab mode signaled in ETI if the
             // dab mode was not set in contructor
             if (myTFDurationMs == 0) {
@@ -352,7 +351,6 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
             etiLog.level(debug) << "OutputUHD: UHD initialising complete";
         }
 
-
         if (lastLen != dataIn->getLength()) {
             // I expect that this never happens.
             etiLog.level(emerg) <<
@@ -361,6 +359,10 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
             throw std::runtime_error("Non-constant input length!");
         }
 
+        uwd.sourceContainsTimestamp = myConf.enableSync &&
+            myEtiReader->sourceContainsTimestamp();
+
+
         if (uwd.check_gpsfix) {
             try {
                 check_gps();
@@ -400,27 +402,34 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
         if (!uwd.running) {
             worker.stop();
             first_run = true;
-            if (uwd.failed_due_to_fct) {
-                throw fct_discontinuity_error();
-            }
-            else {
-                etiLog.level(error) <<
-                    "OutputUHD: Error, UHD worker failed";
-                throw std::runtime_error("UHD worker failed");
-            }
+
+            etiLog.level(error) <<
+                "OutputUHD: Error, UHD worker failed";
+            throw std::runtime_error("UHD worker failed");
         }
 
-        uwd.sourceContainsTimestamp = myConf.enableSync &&
-            myEtiReader->sourceContainsTimestamp();
+        if (frame.ts.fct == -1) {
+            etiLog.level(info) <<
+                "OutputUHD: dropping one frame with invalid FCT";
+        }
+        else if (last_fct != -1 and
+                (last_fct + fct_increment) % 250 != frame.ts.fct) {
+            worker.stop();
+            first_run = true;
+            throw fct_discontinuity_error();
+        }
+        else {
+            while (true) {
+                if (uwd.frames.size() > FRAMES_MAX_SIZE) {
+                    usleep(10000); // 10ms
+                }
 
-        while (true) {
-            if (uwd.frames.size() > FRAMES_MAX_SIZE) {
-                usleep(10000); // 10ms
+                uwd.frames.push(frame);
+                break;
             }
-
-            uwd.frames.push(frame);
-            break;
         }
+
+        //last_fct = frame.ts.fct; // TODO
     }
 
     return dataIn->getLength();
@@ -604,24 +613,14 @@ void OutputUHD::check_gps()
 
 void UHDWorker::process_errhandler()
 {
-    try {
-        process();
-    }
-    catch (fct_discontinuity_error& e) {
-        etiLog.level(warn) << e.what();
-        uwd->failed_due_to_fct = true;
-    }
-
+    process();
     uwd->running = false;
     etiLog.level(warn) << "UHD worker terminated";
 }
 
 void UHDWorker::process()
 {
-    int workerbuffer = 0;
-    tx_second        = 0;
-    pps_offset       = 0.0;
-    last_pps         = 2.0;
+    last_tx_time_initialised = false;
 
 #if FAKE_UHD == 0
     uhd::stream_args_t stream_args("fc32"); //complex floats
@@ -631,13 +630,10 @@ void UHDWorker::process()
     md.start_of_burst = false;
     md.end_of_burst   = false;
 
-    expected_next_fct = -1;
-
     num_underflows   = 0;
     num_late_packets = 0;
 
     while (uwd->running) {
-        fct_discontinuity = false;
         md.has_time_spec  = false;
         md.time_spec      = uhd::time_spec_t(0.0);
 
@@ -645,9 +641,6 @@ void UHDWorker::process()
         uwd->frames.wait_and_pop(frame);
 
         handle_frame(&frame);
-
-        // swap buffers
-        workerbuffer = (workerbuffer + 1) % 2;
     }
 }
 
@@ -656,32 +649,6 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
     // Transmit timeout
     static const double tx_timeout = 20.0;
 
-    pps_offset = frame->ts.timestamp_pps_offset;
-
-    // Tx second from MNSC
-    tx_second = frame->ts.timestamp_sec;
-
-    /* Verify that the FCT value is correct. If we miss one transmission
-     * frame we must interrupt UHD and resync to the timestamps
-     */
-    if (frame->ts.fct == -1) {
-        etiLog.level(info) <<
-            "OutputUHD: dropping one frame with invalid FCT";
-        return;
-    }
-    if (expected_next_fct != -1) {
-        if (expected_next_fct != (int)frame->ts.fct) {
-            etiLog.level(warn) <<
-                "OutputUHD: Incorrect expect fct " << frame->ts.fct <<
-                ", expected " << expected_next_fct;
-
-            fct_discontinuity = true;
-            throw fct_discontinuity_error();
-        }
-    }
-
-    expected_next_fct = (frame->ts.fct + uwd->fct_increment) % 250;
-
     // Check for ref_lock
     if (uwd->check_refclk_loss) {
         try {
@@ -704,9 +671,13 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
     }
 
     double usrp_time = uwd->myUsrp->get_time_now().get_real_secs();
-
+    bool timestamp_discontinuity = false;
 
     if (uwd->sourceContainsTimestamp) {
+        // Tx time from MNSC and TIST
+        uint32_t tx_second = frame->ts.timestamp_sec;
+        uint32_t tx_pps    = frame->ts.timestamp_pps;
+
         if (!frame->ts.timestamp_valid) {
             /* We have not received a full timestamp through
              * MNSC. We sleep through the frame.
@@ -714,11 +685,43 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
             etiLog.level(info) <<
                 "OutputUHD: Throwing sample " << frame->ts.fct <<
                 " away: incomplete timestamp " << tx_second <<
-                " + " << pps_offset;
+                " / " << tx_pps;
             usleep(20000); //TODO should this be TM-dependant ?
             return;
         }
 
+        if (last_tx_time_initialised) {
+            const size_t sizeIn = frame->buf.size() / sizeof(complexf);
+            uint64_t increment = (uint64_t)sizeIn * 16384000ul /
+                                 (uint64_t)uwd->sampleRate;
+                                  // samps  * ticks/s  / (samps/s)
+                                  // (samps * ticks * s) / (s * samps)
+                                  // ticks
+
+            uint32_t expected_sec = last_tx_second + increment / 16384000ul;
+            uint32_t expected_pps = last_tx_pps + increment % 16384000ul;
+
+            while (expected_pps > 16384000) {
+                expected_sec++;
+                expected_pps -= 16384000;
+            }
+
+            if (expected_sec != tx_second or
+                    expected_pps != tx_pps) {
+                etiLog.level(warn) << "OutputUHD: tx time unexpected!" <<
+                " exp " << expected_sec << " " << expected_pps <<
+                " got " << tx_second << " " << tx_pps;
+
+                timestamp_discontinuity = true;
+            }
+        }
+
+        last_tx_second = tx_second;
+        last_tx_pps    = tx_pps;
+        last_tx_time_initialised = true;
+
+        double pps_offset = tx_pps / 16384000.0;
+
         md.has_time_spec = true;
         md.time_spec = uhd::time_spec_t(tx_second, pps_offset);
 
@@ -760,9 +763,9 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
         }
     }
 
-    tx_frame(frame);
+    tx_frame(frame, timestamp_discontinuity);
 
-    if (last_pps > pps_offset) {
+    if (last_usrp_time + 1.0 < usrp_time) {
         if (num_underflows or num_late_packets) {
             etiLog.log(info,
                     "OutputUHD status (usrp time: %f): "
@@ -774,10 +777,10 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
         num_late_packets = 0;
     }
 
-    last_pps = pps_offset;
+    last_usrp_time = usrp_time;
 }
 
-void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame)
+void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame, bool ts_update)
 {
     const double tx_timeout = 20.0;
     const size_t sizeIn = frame->buf.size() / sizeof(complexf);
@@ -793,13 +796,13 @@ void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame)
     while (uwd->running && !uwd->muting && (num_acc_samps < sizeIn)) {
         size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);
 
+        uhd::tx_metadata_t md_tx = md;
+
         //ensure the the last packet has EOB set if the timestamps has been
         //refreshed and need to be reconsidered.
-        //Also, if we saw that the FCT did not increment as expected, which
-        //could be due to a lost incoming packet.
-        md.end_of_burst = (
+        md_tx.end_of_burst = (
                 uwd->sourceContainsTimestamp &&
-                (frame->ts.timestamp_refresh || fct_discontinuity) &&
+                (frame->ts.timestamp_refresh or ts_update) &&
                 samps_to_send <= usrp_max_num_samps );
 
 
@@ -811,13 +814,13 @@ void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame)
         //send a single packet
         size_t num_tx_samps = myTxStream->send(
                 &in_data[num_acc_samps],
-                samps_to_send, md, tx_timeout);
+                samps_to_send, md_tx, tx_timeout);
 #endif
 
         num_acc_samps += num_tx_samps;
 
-        md.time_spec = uhd::time_spec_t(tx_second, pps_offset)
-            + uhd::time_spec_t(0, num_tx_samps/uwd->sampleRate);
+        md_tx.time_spec = md.time_spec +
+            uhd::time_spec_t(0, num_tx_samps/uwd->sampleRate);
 
         if (num_tx_samps == 0) {
             etiLog.log(warn,
diff --git a/src/OutputUHD.h b/src/OutputUHD.h
index 8cbfb3d..f725268 100644
--- a/src/OutputUHD.h
+++ b/src/OutputUHD.h
@@ -101,7 +101,6 @@ enum refclk_lock_loss_behaviour_t { CRASH, IGNORE };
 
 struct UHDWorkerData {
     bool running;
-    bool failed_due_to_fct;
 
 #if FAKE_UHD == 0
     uhd::usrp::multi_usrp::sptr myUsrp;
@@ -129,11 +128,6 @@ struct UHDWorkerData {
 
     // What to do when the reference clock PLL loses lock
     refclk_lock_loss_behaviour_t refclk_lock_loss_behaviour;
-
-    // What transmission mode we're using defines by how
-    // much the FCT should increment for each
-    // transmission frame.
-    int fct_increment;
 };
 
 
@@ -143,7 +137,6 @@ class UHDWorker {
             uwd = uhdworkerdata;
 
             uwd->running = true;
-            uwd->failed_due_to_fct = false;
             uhd_thread = boost::thread(&UHDWorker::process_errhandler, this);
         }
 
@@ -160,17 +153,18 @@ class UHDWorker {
         int num_underflows;
         int num_late_packets;
 
-        bool fct_discontinuity;
-        int expected_next_fct;
         uhd::tx_metadata_t md;
-        time_t tx_second;
-        double pps_offset;
-        double last_pps;
+        bool     last_tx_time_initialised;
+        uint32_t last_tx_second;
+        uint32_t last_tx_pps;
+
+        // Used to print statistics once a second
+        double   last_usrp_time;
 
         void print_async_metadata(const struct UHDWorkerFrameData *frame);
 
         void handle_frame(const struct UHDWorkerFrameData *frame);
-        void tx_frame(const struct UHDWorkerFrameData *frame);
+        void tx_frame(const struct UHDWorkerFrameData *frame, bool ts_update);
 
         struct UHDWorkerData *uwd;
         boost::thread uhd_thread;
@@ -279,6 +273,13 @@ class OutputUHD: public ModOutput, public RemoteControllable {
         boost::unique_future<bool> gps_fix_future;
         boost::thread gps_fix_task;
 
+
+        // What transmission mode we're using defines by how
+        // much the FCT should increment for each
+        // transmission frame.
+        int fct_increment;
+        int last_fct;
+
         // Wait time in seconds to get fix
         static const int initial_gps_fix_wait = 180;
 
diff --git a/src/TimestampDecoder.cpp b/src/TimestampDecoder.cpp
index 2cfb281..5f93407 100644
--- a/src/TimestampDecoder.cpp
+++ b/src/TimestampDecoder.cpp
@@ -46,13 +46,14 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
     /* Push new timestamp into queue */
     ts_queued->timestamp_valid = full_timestamp_received_mnsc;
     ts_queued->timestamp_sec = time_secs;
-    ts_queued->timestamp_pps_offset = time_pps;
+    ts_queued->timestamp_pps = time_pps;
     ts_queued->fct = latestFCT;
 
     ts_queued->timestamp_refresh = offset_changed;
     offset_changed = false;
 
-    MDEBUG("time_secs=%d, time_pps=%f\n", time_secs, time_pps);
+    MDEBUG("time_secs=%d, time_pps=%f\n", time_secs,
+            (double)time_pps / 16384000.0);
     *ts_queued += timestamp_offset;
 
     queue_timestamps.push(ts_queued);
@@ -67,7 +68,7 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
         /* Return invalid timestamp until the queue is full */
         ts.timestamp_valid = false;
         ts.timestamp_sec = 0;
-        ts.timestamp_pps_offset = 0;
+        ts.timestamp_pps = 0;
         ts.timestamp_refresh = false;
         ts.fct = -1;
     }
@@ -168,9 +169,9 @@ void TimestampDecoder::updateTimestampSeconds(uint32_t secs)
     }
 }
 
-void TimestampDecoder::updateTimestampPPS(double pps)
+void TimestampDecoder::updateTimestampPPS(uint32_t pps)
 {
-    MDEBUG("TimestampDecoder::updateTimestampPPS(%f)\n", pps);
+    MDEBUG("TimestampDecoder::updateTimestampPPS(%f)\n", (double)pps / 16384000.0);
 
     if (time_pps > pps) // Second boundary crossed
     {
@@ -188,7 +189,7 @@ void TimestampDecoder::updateTimestampPPS(double pps)
 void TimestampDecoder::updateTimestampEti(
         int framephase,
         uint16_t mnsc,
-        double pps,
+        uint32_t pps, // In units of 1/16384000 s
         int32_t fct)
 {
     updateTimestampPPS(pps);
diff --git a/src/TimestampDecoder.h b/src/TimestampDecoder.h
index 5bb5ec8..09b4050 100644
--- a/src/TimestampDecoder.h
+++ b/src/TimestampDecoder.h
@@ -43,7 +43,7 @@ struct frame_timestamp
     int32_t fct;
 
     uint32_t timestamp_sec;
-    double timestamp_pps_offset;
+    uint32_t timestamp_pps; // In units of 1/16384000 s
     bool timestamp_valid;
     bool timestamp_refresh;
 
@@ -51,7 +51,7 @@ struct frame_timestamp
     {
         if (this != &rhs) {
             this->timestamp_sec = rhs.timestamp_sec;
-            this->timestamp_pps_offset = rhs.timestamp_pps_offset;
+            this->timestamp_pps = rhs.timestamp_pps;
             this->timestamp_valid = rhs.timestamp_valid;
             this->timestamp_refresh = rhs.timestamp_refresh;
             this->fct = rhs.fct;
@@ -66,11 +66,11 @@ struct frame_timestamp
         offset_pps = modf(diff, &offset_secs);
 
         this->timestamp_sec += lrintf(offset_secs);
-        this->timestamp_pps_offset += offset_pps;
+        this->timestamp_pps += lrintf(offset_pps / 16384000.0);
 
-        while (this->timestamp_pps_offset > 1)
+        while (this->timestamp_pps > 16384000)
         {
-            this->timestamp_pps_offset -= 1.0;
+            this->timestamp_pps -= 16384000;
             this->timestamp_sec += 1;
         };
         return *this;
@@ -83,12 +83,16 @@ struct frame_timestamp
         return ts;
     }
 
+    double pps_offset() const {
+        return timestamp_pps / 16384000.0;
+    }
+
     void print(const char* t)
     {
         fprintf(stderr,
                 "%s <struct frame_timestamp(%s, %d, %.9f, %d)>\n",
                 t, this->timestamp_valid ? "valid" : "invalid",
-                 this->timestamp_sec, this->timestamp_pps_offset,
+                 this->timestamp_sec, pps_offset(),
                  this->fct);
     }
 };
@@ -135,7 +139,7 @@ class TimestampDecoder : public RemoteControllable
         void updateTimestampEti(
                 int framephase,
                 uint16_t mnsc,
-                double pps,
+                uint32_t pps, // In units of 1/16384000 s
                 int32_t fct);
 
         /*********** REMOTE CONTROL ***************/
@@ -160,9 +164,11 @@ class TimestampDecoder : public RemoteControllable
 
         /* Each frame contains the TIST field with the PPS offset.
          * For each frame, this function must be called to update
-         * the timestamp
+         * the timestamp.
+         *
+         * pps is in units of 1/16384000 s
          */
-        void updateTimestampPPS(double pps);
+        void updateTimestampPPS(uint32_t pps);
 
         /* Update the timestamp when a full set of MNSC data is
          * known. This function can be called at most every four
@@ -173,7 +179,7 @@ class TimestampDecoder : public RemoteControllable
         struct tm temp_time;
         uint32_t time_secs;
         int32_t latestFCT;
-        double time_pps;
+        uint32_t time_pps;
         double& timestamp_offset;
         unsigned m_tist_delay_stages;
         int inhibit_second_update;