Replace manual correlation by FFT

1 files changed, 49 insertions, 10 deletions

diff --git a/main.cpp b/main.cpp
index 4477ef5..3a1387d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -40,7 +40,7 @@ void sig_int_handler(int) {
     running = false;
 }
 
-size_t read_samples(FILE* fd, std::vector<complexf>& samples, size_t count)
+size_t read_samples_from_file(FILE* fd, std::vector<complexf>& samples, size_t count)
 {
     if (samples.size() < count) {
         MDEBUG("HAD TO RESIZE BUFFER!\n");
@@ -87,14 +87,12 @@ void find_peak_correlation()
     FILE* fd = fopen("correlation.debug", "w");
 
     while (running) {
-        const size_t max_offset = 10000; // 4.8ms at 2048000
-        if (aligner.ready(max_offset)) {
-            const size_t correlation_length = 1000;
-            std::vector<complexf> correlations(max_offset);
+        const size_t correlation_length = 16 * 1024; // 8ms at 2048000
+        if (aligner.ready(correlation_length)) {
             double max_norm = 0.0;
             size_t pos_max = 0;
 
-            auto result = aligner.crosscorrelate(max_offset, correlation_length);
+            auto result = aligner.crosscorrelate(correlation_length);
             auto& xcs = result.correlation;
 
             fprintf(fd, "Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
@@ -162,13 +160,51 @@ int main(int argc, char **argv)
 
     std::string uri = argv[1];
 
-    OutputUHD output_uhd(txgain, rxgain, samplerate);
-
     zmq::context_t ctx;
     zmq::socket_t zmq_sock(ctx, ZMQ_SUB);
 
     FILE* fd = nullptr;
-    if (uri.find("tcp://") != 0) {
+    if (uri == "test") {
+        FILE* fd_rx = fopen("rx.debug", "r");
+        FILE* fd_tx = fopen("tx.debug", "r");
+
+        const size_t len = 64;
+
+        std::vector<complexf> rx_samples(len*1024);
+        std::vector<complexf> tx_samples(len*1024);
+
+        size_t num_rx_samples = fread(&rx_samples.front(), sizeof(complexf), len, fd_rx);
+        size_t num_tx_samples = fread(&tx_samples.front(), sizeof(complexf), len, fd_tx);
+
+        aligner.push_rx_samples(&rx_samples.front(), num_rx_samples, 1);
+        aligner.push_tx_samples(&tx_samples.front(), num_tx_samples, 1);
+
+        aligner.debug();
+        const size_t correlation_length = 16 * 1024;
+        double max_norm = 0.0;
+        size_t pos_max = 0;
+
+        auto result = aligner.crosscorrelate(correlation_length);
+        auto& xcs = result.correlation;
+        for (size_t offset = 0; offset < xcs.size(); offset++) {
+            complexf xc = xcs[offset];
+            fprintf(fd, "%f ", std::norm(xc));
+            if (std::norm(xc) >= max_norm) {
+                max_norm = std::norm(xc);
+                pos_max = offset;
+            }
+        }
+        MDEBUG("Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
+                std::sqrt(max_norm),
+                (double)pos_max / (double)samplerate * 1000.0,
+                10*std::log(result.rx_power),
+                10*std::log(result.tx_power),
+                result.rx_timestamp,
+                result.tx_timestamp);
+
+        return 0;
+    }
+    else if (uri.find("tcp://") != 0) {
         fd = fopen(uri.c_str(), "rb");
         if (!fd) {
             MDEBUG("Could not open file\n");
@@ -180,6 +216,9 @@ int main(int argc, char **argv)
         zmq_sock.setsockopt(ZMQ_SUBSCRIBE, NULL, 0);
     }
 
+    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;
@@ -199,7 +238,7 @@ int main(int argc, char **argv)
         double first_sample_time = 0;
 
         if (fd) {
-            samps_read = read_samples(fd, 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);
         }