Move definition of TX timestamp and fix bug after RX reset

5 files changed, 50 insertions, 34 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index dbb68c7..b6afc2a 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -72,10 +72,25 @@ void AlignSample::push_rx_samples(complexf* samps, size_t len, double first_samp
     }
     else if (m_first_rx_sample_time != 0) {
         // We have previously received samples with valid timestamp
+        double delta = m_rx_sample_time(m_rxsamples.size()) - first_sample_time;
+        if (std::fabs(delta) > 1e-3) {
+            fprintf(stderr, "RX sample time %f expected %f. Delta of %f(%zu)."
+                    " Resetting RX\n",
+                m_rx_sample_time(m_rxsamples.size()),
+                first_sample_time,
+                delta,
+                (size_t)std::fabs(delta * (double)samplerate));
+
+            m_first_rx_sample_time = 0;
+            m_num_rx_samples_dropped = 0;
+            m_rxsamples.clear();
+        }
+
         std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
     }
     else if (first_sample_time == 0) {
         MDEBUG("RX timestamp missing\n");
+        throw std::runtime_error("RX timestamp missing");
     }
 }
 
@@ -141,8 +156,8 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
         std::copy(m_txsamples.begin(), m_txsamples.begin() + len, tx_fft_in.begin());
         // the other half of the buffers are set to 0
 
-        m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + len);
-        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len);
+        m_drop_rx_samples(len);
+        m_drop_tx_samples(len);
 
         rx_ts = m_rx_sample_time();
         tx_ts = m_tx_sample_time();
@@ -199,11 +214,8 @@ void AlignSample::consume(size_t samples)
 {
     std::lock_guard<std::mutex> lock(m_mutex);
     if (align() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
-        m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
-        m_num_rx_samples_dropped += samples;
-
-        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
-        m_num_tx_samples_dropped += samples;
+        m_drop_rx_samples(samples);
+        m_drop_tx_samples(samples);
     }
 }
 
@@ -220,10 +232,7 @@ bool AlignSample::align()
             return false;
         }
 
-        m_rxsamples.erase(
-                m_rxsamples.begin(), m_rxsamples.begin() + rx_samples_to_skip);
-
-        m_num_rx_samples_dropped += rx_samples_to_skip;
+        m_drop_rx_samples(rx_samples_to_skip);
         return true;
     }
     else if (m_rx_sample_time() > m_tx_sample_time()) {
@@ -234,10 +243,7 @@ bool AlignSample::align()
             return false;
         }
 
-        m_txsamples.erase(
-                m_txsamples.begin(), m_txsamples.begin() + tx_samples_to_skip);
-
-        m_num_tx_samples_dropped += tx_samples_to_skip;
+        m_drop_tx_samples(tx_samples_to_skip);
         return true;
     }
     return false;
diff --git a/AlignSample.hpp b/AlignSample.hpp
index 78c430f..5757ff9 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -88,14 +88,24 @@ class AlignSample {
     private:
         bool align();
 
-        double m_rx_sample_time() const {
+        double m_rx_sample_time(size_t offset = 0) const {
             return m_first_rx_sample_time +
-                (double)m_num_rx_samples_dropped / samplerate;
+                (double)(m_num_rx_samples_dropped + offset) / (double)samplerate;
         }
 
-        double m_tx_sample_time() const {
+        double m_tx_sample_time(size_t offset = 0) const {
             return m_first_tx_sample_time +
-                (double)m_num_tx_samples_dropped / samplerate;
+                (double)(m_num_tx_samples_dropped + offset) / (double)samplerate;
+        }
+
+        void m_drop_tx_samples(size_t samples) {
+            m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
+            m_num_tx_samples_dropped += samples;
+        }
+
+        void m_drop_rx_samples(size_t samples) {
+            m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
+            m_num_rx_samples_dropped += samples;
         }
 
         std::mutex m_mutex;
diff --git a/OutputUHD.cpp b/OutputUHD.cpp
index a80f5b9..9536ebd 100644
--- a/OutputUHD.cpp
+++ b/OutputUHD.cpp
@@ -67,14 +67,13 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) :
     m_usrp->set_rx_gain(m_rxgain);
     MDEBUG("OutputUHD:Actual RX Gain: %f ...\n", m_usrp->get_rx_gain());
 
-    double tx_time = m_usrp->get_time_now().get_real_secs();
+    const double usrp_time = m_usrp->get_time_now().get_real_secs();
     MDEBUG("OutputUHD: USRP time %f\n",
-            tx_time);
+            usrp_time);
 
     md.start_of_burst = false;
     md.end_of_burst   = false;
     md.has_time_spec  = true;
-    md.time_spec      = uhd::time_spec_t(tx_time + 2);
 
     uhd::stream_args_t stream_args("fc32"); //complex floats
     myTxStream = m_usrp->get_tx_stream(stream_args);
@@ -89,13 +88,13 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) :
 
 }
 
-size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double *first_sample_time)
+size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double transmit_time)
 {
     const double tx_timeout = 20.0;
 
     size_t usrp_max_num_samps = myTxStream->get_max_num_samps();
 
-    *first_sample_time = md.time_spec.get_real_secs();
+    md.time_spec      = uhd::time_spec_t(transmit_time);
 
     size_t num_acc_samps = 0; //number of accumulated samples
     while (num_acc_samps < sizeIn) {
diff --git a/OutputUHD.hpp b/OutputUHD.hpp
index f1d164e..b3169c6 100644
--- a/OutputUHD.hpp
+++ b/OutputUHD.hpp
@@ -33,7 +33,7 @@ class OutputUHD {
     public:
         OutputUHD(double txgain, double rxgain, double samplerate);
 
-        size_t Transmit(const complexf *samples, size_t sizeIn, double *first_sample_time);
+        size_t Transmit(const complexf *samples, size_t sizeIn, double transmit_time);
         ssize_t Receive(complexf *samples, size_t sizeIn, double *first_sample_time);
 
     private:
diff --git a/main.cpp b/main.cpp
index d2ab5f8..4379a3e 100644
--- a/main.cpp
+++ b/main.cpp
@@ -259,13 +259,15 @@ int main(int argc, char **argv)
     std::thread correlator_thread(find_peak_correlation);
 
     do {
-        double first_sample_time = 0;
+        const double first_sample_time = 4.0;
+        const double sample_time = first_sample_time + (double)total_samps_read / (double)samplerate;
 
         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);
+
+            sent = output_uhd.Transmit(&input_samples.front(), samps_read, sample_time);
+            aligner.push_tx_samples(&input_samples.front(), samps_read, sample_time);
         }
         else {
             zmq::message_t msg;
@@ -280,15 +282,14 @@ int main(int argc, char **argv)
             }
             samps_read = msg.size() / sizeof(complexf);
 
-            sent = output_uhd.Transmit((complexf*)msg.data(), samps_read, &first_sample_time);
-
-            aligner.push_tx_samples((complexf*)msg.data(), samps_read, first_sample_time);
+            sent = output_uhd.Transmit((complexf*)msg.data(), samps_read, sample_time);
 
+            aligner.push_tx_samples((complexf*)msg.data(), samps_read, sample_time);
         }
 
-        if (first_sample_time - last_print_time > 1) {
-            //MDEBUG("Tx %zu samples at t=%f\n", samps_read, first_sample_time);
-            last_print_time = first_sample_time;
+        if (sample_time - last_print_time > 1) {
+            //MDEBUG("Tx %zu samples at t=%f\n", samps_read, sample_time);
+            last_print_time = sample_time;
         }
 
         total_samps_read += samps_read;