1 files changed, 21 insertions, 15 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;