Create dedicated thread for correlator

1 files changed, 42 insertions, 39 deletions

diff --git a/main.cpp b/main.cpp
index a043e01..5304e5a 100644
--- a/main.cpp
+++ b/main.cpp
@@ -93,24 +93,34 @@ class AlignSample {
             MDEBUG("  TX: %f %zu\n", m_tx_sample_time, m_txsamples.size());
         }
 
-        std::pair<bool, complexf> crosscorrelate(size_t offset, size_t len) {
-            complexf xcorr(0, 0);
+        complexf crosscorrelate(size_t offset, size_t len) {
+            std::vector<complexf> rxsamps;
+            std::vector<complexf> txsamps;
+
+            // Do a quick copy, so as to free the mutex
+            {
+                std::lock_guard<std::mutex> lock(m_mutex);
 
-            if (m_rxsamples.size() < len or
-                m_txsamples.size() < len + offset) {
-                return {false, 0};
+                if (m_rxsamples.size() < len or
+                        m_txsamples.size() < len + offset) {
+                    return 0;
+                }
+
+                std::copy(m_rxsamples.begin(), m_rxsamples.begin() + len, std::back_inserter(rxsamps));
+                std::copy(m_txsamples.begin(), m_txsamples.begin() + len + offset, std::back_inserter(txsamps));
             }
 
+            complexf xcorr(0, 0);
+
             for (size_t i = 0; i < len; i++) {
-                xcorr += m_rxsamples[i] * std::conj(m_txsamples[i+offset]);
+                xcorr += rxsamps[i] * std::conj(txsamps[i+offset]);
             }
-            return {true, xcorr};
+            return xcorr;
         }
 
     private:
         bool aligned() {
             std::lock_guard<std::mutex> lock(m_mutex);
-            debug();
 
             if (std::abs(m_rx_sample_time - m_tx_sample_time) < 1e-6) {
                 return true;
@@ -176,28 +186,30 @@ size_t do_receive(OutputUHD* output_uhd)
 
 void find_peak_correlation()
 {
-    if (aligner.ready()) {
-        const size_t max_offset = 1000; // 488us at 2048000
-        std::vector<complexf> correlations(max_offset);
-        double max_ampl = 0.0;
-        size_t pos_max = 0;
-        for (size_t offset = 0; offset < max_offset; offset++) {
-            auto valid_xc = aligner.crosscorrelate(offset, max_offset);
-
-            if (not valid_xc.first) { return; }
-
-            auto xc = valid_xc.second;
-            correlations[offset] = xc;
-
-            if (std::abs(xc) >= max_ampl) {
-                max_ampl = std::abs(xc);
-                pos_max = offset;
+    while (running) {
+        if (aligner.ready()) {
+            const size_t max_offset = 1000; // 488us at 2048000
+            std::vector<complexf> correlations(max_offset);
+            double max_ampl = 0.0;
+            size_t pos_max = 0;
+            for (size_t offset = 0; offset < max_offset; offset++) {
+                auto xc = aligner.crosscorrelate(offset, max_offset);
+                correlations[offset] = xc;
+
+                if (std::abs(xc) >= max_ampl) {
+                    max_ampl = std::abs(xc);
+                    pos_max = offset;
+                }
             }
+            MDEBUG("Max correlation is %f at %zu\n", max_ampl, pos_max);
+            std::this_thread::sleep_for(std::chrono::microseconds(1));
+
+            aligner.debug();
+        }
+        else {
+            MDEBUG("Not aligned\n");
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
-        MDEBUG("Max correlation is %f at %zu\n", max_ampl, pos_max);
-    }
-    else {
-        MDEBUG("Not aligned\n");
     }
 }
 
@@ -254,7 +266,7 @@ int main(int argc, char **argv)
 
     std::thread receive_thread(do_receive, &output_uhd);
 
-    auto fut_corr = std::async(std::launch::async, find_peak_correlation);
+    std::thread correlator_thread(find_peak_correlation);
 
     do {
         double first_sample_time = 0;
@@ -289,14 +301,6 @@ int main(int argc, char **argv)
         }
 
         total_samps_read += samps_read;
-
-        if (aligner.ready()) {
-            if (fut_corr.valid()) {
-                fut_corr.get();
-
-                fut_corr = std::async(std::launch::async, find_peak_correlation);
-            }
-        }
     }
     while (samps_read and sent and running);
     MDEBUG("Leaving main loop with running=%d\n", running ? 1 : 0);
@@ -304,8 +308,7 @@ int main(int argc, char **argv)
     running = false;
 
     receive_thread.join();
-
-    aligner.debug();
+    correlator_thread.join();
 
 }