Add rxgain, do less copying for correlation

1 files changed, 51 insertions, 20 deletions

diff --git a/main.cpp b/main.cpp
index 5304e5a..3e83074 100644
--- a/main.cpp
+++ b/main.cpp
@@ -84,6 +84,7 @@ class AlignSample {
         }
 
         bool ready() {
+            std::lock_guard<std::mutex> lock(m_mutex);
             return aligned() and m_rxsamples.size() > 8000 and m_txsamples.size() > 8000;
         }
 
@@ -93,7 +94,7 @@ class AlignSample {
             MDEBUG("  TX: %f %zu\n", m_tx_sample_time, m_txsamples.size());
         }
 
-        complexf crosscorrelate(size_t offset, size_t len) {
+        std::vector<complexf> crosscorrelate(size_t max_offset, size_t len) {
             std::vector<complexf> rxsamps;
             std::vector<complexf> txsamps;
 
@@ -102,26 +103,41 @@ class AlignSample {
                 std::lock_guard<std::mutex> lock(m_mutex);
 
                 if (m_rxsamples.size() < len or
-                        m_txsamples.size() < len + offset) {
-                    return 0;
+                    m_txsamples.size() < len + max_offset) {
+                    return {};
                 }
 
                 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));
+                std::copy(m_txsamples.begin(), m_txsamples.begin() + len + max_offset, std::back_inserter(txsamps));
             }
 
-            complexf xcorr(0, 0);
+            std::vector<complexf> xcorrs(max_offset);
+
+            for (size_t offset = 0; offset < max_offset; offset++) {
+                complexf xcorr(0, 0);
 
-            for (size_t i = 0; i < len; i++) {
-                xcorr += rxsamps[i] * std::conj(txsamps[i+offset]);
+                for (size_t i = 0; i < len; i++) {
+                    xcorr += rxsamps[i] * std::conj(txsamps[i+offset]);
+                }
+                xcorrs[offset] = xcorr;
             }
-            return xcorr;
+            return xcorrs;
         }
 
-    private:
-        bool aligned() {
+        void consume(size_t samples)
+        {
             std::lock_guard<std::mutex> lock(m_mutex);
+            if (aligned() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
+                m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
+                m_rx_sample_time += (double)samples / samplerate;
+
+                m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
+                m_tx_sample_time += (double)samples / samplerate;
+            }
+        }
 
+    private:
+        bool aligned() {
             if (std::abs(m_rx_sample_time - m_tx_sample_time) < 1e-6) {
                 return true;
             }
@@ -175,7 +191,7 @@ size_t do_receive(OutputUHD* output_uhd)
         total_received += received;
 
         if (first_sample_time - last_print_time > 1) {
-            MDEBUG("Rx %zu samples at t=%f\n", received, first_sample_time);
+            //MDEBUG("Rx %zu samples at t=%f\n", received, first_sample_time);
             last_print_time = first_sample_time;
         }
     }
@@ -188,14 +204,16 @@ void find_peak_correlation()
 {
     while (running) {
         if (aligner.ready()) {
-            const size_t max_offset = 1000; // 488us at 2048000
+            const size_t max_offset = 100000; // 48ms at 2048000
+            const size_t correlation_length = 100;
             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;
 
+            auto xcs = aligner.crosscorrelate(max_offset, correlation_length);
+
+            for (size_t offset = 0; offset < xcs.size(); offset++) {
+                complexf xc = xcs[offset];
                 if (std::abs(xc) >= max_ampl) {
                     max_ampl = std::abs(xc);
                     pos_max = offset;
@@ -204,11 +222,14 @@ void find_peak_correlation()
             MDEBUG("Max correlation is %f at %zu\n", max_ampl, pos_max);
             std::this_thread::sleep_for(std::chrono::microseconds(1));
 
+            // Eat much more than we correlate, because correlation is slow
+            aligner.consume(2048000);
             aligner.debug();
         }
         else {
-            MDEBUG("Not aligned\n");
-            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            MDEBUG("Waiting for correlation\n");
+            aligner.debug();
+            std::this_thread::sleep_for(std::chrono::seconds(1));
         }
     }
 }
@@ -217,8 +238,9 @@ void find_peak_correlation()
 int main(int argc, char **argv)
 {
     double txgain = 0;
+    double rxgain = 0;
 
-    if (argc == 3) {
+    if (argc >= 3) {
         txgain = strtod(argv[2], nullptr);
         if (!(0 <= txgain and txgain < 80)) {
             MDEBUG("txgain wrong: %f\n", txgain);
@@ -226,7 +248,16 @@ int main(int argc, char **argv)
         }
     }
 
+    if (argc >= 4) {
+        rxgain = strtod(argv[3], nullptr);
+        if (!(0 <= rxgain and rxgain < 80)) {
+            MDEBUG("rxgain wrong: %f\n", rxgain);
+            return -1;
+        }
+    }
+
     MDEBUG("TX Gain is %f\n", txgain);
+    MDEBUG("RX Gain is %f\n", rxgain);
 
     if (argc < 2) {
         MDEBUG("Require input file or url\n");
@@ -235,7 +266,7 @@ int main(int argc, char **argv)
 
     std::string uri = argv[1];
 
-    OutputUHD output_uhd(txgain, samplerate);
+    OutputUHD output_uhd(txgain, rxgain, samplerate);
 
     zmq::context_t ctx;
     zmq::socket_t zmq_sock(ctx, ZMQ_SUB);
@@ -296,7 +327,7 @@ int main(int argc, char **argv)
         }
 
         if (first_sample_time - last_print_time > 1) {
-            MDEBUG("Tx %zu samples at t=%f\n", samps_read, first_sample_time);
+            //MDEBUG("Tx %zu samples at t=%f\n", samps_read, first_sample_time);
             last_print_time = first_sample_time;
         }