Reset receive timestamps on error

5 files changed, 55 insertions, 36 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index 15dda60..dbb68c7 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -53,10 +53,6 @@ namespace fftw {
 
 void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_sample_time)
 {
-    if (first_sample_time > 5.1 and first_sample_time < 5.5) {
-        fwrite(samps, sizeof(complexf), len, fd_tx);
-    }
-
     std::lock_guard<std::mutex> lock(m_mutex);
     std::copy(samps, samps + len, std::back_inserter(m_txsamples));
 
@@ -67,16 +63,28 @@ void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_samp
 
 void AlignSample::push_rx_samples(complexf* samps, size_t len, double first_sample_time)
 {
-    if (first_sample_time > 5.1 and first_sample_time < 5.5) {
-        fwrite(samps, sizeof(complexf), len, fd_rx);
-    }
-
     std::lock_guard<std::mutex> lock(m_mutex);
-    std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
+    if (m_first_rx_sample_time == 0 and first_sample_time != 0) {
+        std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
 
-    if (m_first_rx_sample_time == 0) {
         m_first_rx_sample_time = first_sample_time;
+        m_num_rx_samples_dropped = 0;
+    }
+    else if (m_first_rx_sample_time != 0) {
+        // We have previously received samples with valid timestamp
+        std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
     }
+    else if (first_sample_time == 0) {
+        MDEBUG("RX timestamp missing\n");
+    }
+}
+
+void AlignSample::reset_rx()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    m_first_rx_sample_time = 0;
+    m_num_rx_samples_dropped = 0;
+    m_rxsamples.clear();
 }
 
 bool AlignSample::ready(size_t min_samples)
diff --git a/AlignSample.hpp b/AlignSample.hpp
index 20bd2c8..78c430f 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -56,23 +56,29 @@ class AlignSample {
             m_first_tx_sample_time = 0;
             m_num_rx_samples_dropped = 0;
             m_num_tx_samples_dropped = 0;
-
-            fd_rx = fopen("rx.debug", "wb");
-            fd_tx = fopen("tx.debug", "wb");
-
         }
 
         void push_tx_samples(complexf* samps, size_t len, double first_sample_time);
 
         void push_rx_samples(complexf* samps, size_t len, double first_sample_time);
 
+        void reset_rx(void);
+
         bool ready(size_t min_samples);
 
         void debug() {
             std::lock_guard<std::mutex> lock(m_mutex);
             MDEBUG("Aligner\n");
-            MDEBUG("  RX: %f %zu\n", m_rx_sample_time(), m_rxsamples.size());
-            MDEBUG("  TX: %f %zu\n", m_tx_sample_time(), m_txsamples.size());
+            MDEBUG("  RX: %f--%f %zu\n",
+                    m_rx_sample_time(),
+                    m_rx_sample_time() +
+                    (double)m_rxsamples.size() / (double)samplerate,
+                    m_rxsamples.size());
+            MDEBUG("  TX: %f--%f %zu\n",
+                    m_tx_sample_time(),
+                    m_tx_sample_time() +
+                    (double)m_txsamples.size() / (double)samplerate,
+                    m_txsamples.size());
         }
 
         CorrelationResult crosscorrelate(size_t len);
@@ -100,9 +106,6 @@ class AlignSample {
         double m_first_tx_sample_time;
         size_t m_num_tx_samples_dropped;
         std::deque<complexf> m_txsamples;
-
-        FILE* fd_rx;
-        FILE* fd_tx;
 };
 
 
diff --git a/OutputUHD.cpp b/OutputUHD.cpp
index 6800daf..a80f5b9 100644
--- a/OutputUHD.cpp
+++ b/OutputUHD.cpp
@@ -114,21 +114,21 @@ size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double *first
     return num_acc_samps;
 }
 
-size_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time)
+ssize_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time)
 {
     const double rx_timeout = 20.0;
     const size_t usrp_max_num_samps = myRxStream->get_max_num_samps();
     const size_t samps_to_rx = std::min(sizeIn, usrp_max_num_samps);
 
-    uhd::rx_metadata_t md;
-    size_t num_rx_samps = myRxStream->recv(samples, samps_to_rx, md, rx_timeout);
+    uhd::rx_metadata_t md_rx;
+    ssize_t num_rx_samps = myRxStream->recv(samples, samps_to_rx, md_rx, rx_timeout);
 
-    if (md.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE){
-        MDEBUG("RX Error %s\n", md.strerror().c_str());
+    if (md_rx.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE){
+        MDEBUG("RX Error %s\n", md_rx.strerror().c_str());
+        num_rx_samps = -1;
     }
-
-    if (md.has_time_spec) {
-        *first_sample_time = md.time_spec.get_real_secs();
+    else if (md_rx.has_time_spec) {
+        *first_sample_time = md_rx.time_spec.get_real_secs();
     }
     else {
         *first_sample_time = 0;
diff --git a/OutputUHD.hpp b/OutputUHD.hpp
index 37d8397..f1d164e 100644
--- a/OutputUHD.hpp
+++ b/OutputUHD.hpp
@@ -34,7 +34,7 @@ class OutputUHD {
         OutputUHD(double txgain, double rxgain, double samplerate);
 
         size_t Transmit(const complexf *samples, size_t sizeIn, double *first_sample_time);
-        size_t Receive(complexf *samples, size_t sizeIn, double *first_sample_time);
+        ssize_t Receive(complexf *samples, size_t sizeIn, double *first_sample_time);
 
     private:
         double m_txgain;
diff --git a/main.cpp b/main.cpp
index cd3d96c..d2ab5f8 100644
--- a/main.cpp
+++ b/main.cpp
@@ -68,13 +68,20 @@ size_t do_receive(OutputUHD* output_uhd)
 
     MDEBUG("Starting do_receive\n");
     while (running) {
-        size_t received = output_uhd->Receive(&samps.front(), samps.size(), &first_sample_time);
-        aligner.push_rx_samples(&samps.front(), received, first_sample_time);
-        total_received += received;
-
-        if (first_sample_time - last_print_time > 1) {
-            //MDEBUG("Rx %zu samples at t=%f\n", received, first_sample_time);
-            last_print_time = first_sample_time;
+        ssize_t received = output_uhd->Receive(&samps.front(), samps.size(), &first_sample_time);
+        if (received > 0) {
+            aligner.push_rx_samples(&samps.front(), received, first_sample_time);
+            total_received += received;
+
+            if (first_sample_time - last_print_time > 1) {
+                //MDEBUG("Rx %zu samples at t=%f\n", received, first_sample_time);
+                last_print_time = first_sample_time;
+            }
+        }
+        else {
+            // A receive error occurred that invalidates the RX timestamp
+            MDEBUG("Reset aligner RX\n");
+            aligner.reset_rx();
         }
     }
     MDEBUG("Leaving do_receive\n");
@@ -126,6 +133,7 @@ void find_peak_correlation()
         }
         else {
             MDEBUG("Waiting for correlation\n");
+            aligner.debug();
             std::this_thread::sleep_for(std::chrono::seconds(1));
         }
     }
@@ -236,7 +244,6 @@ int main(int argc, char **argv)
     OutputUHD output_uhd(txgain, rxgain, samplerate);
 
 
-    std::vector<complexf> input_samples(samps_per_buffer);
     size_t samps_read = 0;
     size_t total_samps_read = samps_read;
 
@@ -255,6 +262,7 @@ int main(int argc, char **argv)
         double first_sample_time = 0;
 
         if (fd) {
+            std::vector<complexf> input_samples(samps_per_buffer);
             samps_read = read_samples_from_file(fd, input_samples, samps_per_buffer);
             sent = output_uhd.Transmit(&input_samples.front(), samps_read, &first_sample_time);
             aligner.push_tx_samples(&input_samples.front(), samps_read, first_sample_time);