Create wrapper around FFTW, simplify

3 files changed, 119 insertions, 103 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index 2fcb830..15dda60 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -24,6 +24,33 @@
 #include "AlignSample.hpp"
 #include <cstring>
 
+namespace fftw {
+    class plan {
+        public:
+            template <class T>
+            plan(size_t N, T& in, T& out, int direction)
+            {
+                m_plan = fftwf_plan_dft_1d(N,
+                        reinterpret_cast<fftwf_complex*>(&in.front()),
+                        reinterpret_cast<fftwf_complex*>(&out.front()),
+                        FFTW_FORWARD, FFTW_MEASURE);
+            }
+
+            plan(const plan& other) = delete;
+            const plan& operator=(const plan& other) = delete;
+
+            void execute(void) {
+                fftwf_execute(m_plan);
+            }
+
+            ~plan() {
+                fftwf_destroy_plan(m_plan);
+            }
+        private:
+            fftwf_plan m_plan;
+    };
+}
+
 void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_sample_time)
 {
     if (first_sample_time > 5.1 and first_sample_time < 5.5) {
@@ -69,6 +96,28 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
      */
     const size_t N = 2 * len;
 
+    std::vector<complexf> rx_fft_in(N);
+    std::vector<complexf> rx_fft_out(N);
+    std::vector<complexf> tx_fft_in(N);
+    std::vector<complexf> tx_fft_out(N);
+    std::vector<complexf> ifft_in(N);
+    std::vector<complexf> ifft_out(N);
+
+    fftw::plan rx_fft_plan(N,
+            rx_fft_in,
+            rx_fft_out,
+            FFTW_FORWARD);
+
+    fftw::plan tx_fft_plan(N,
+            tx_fft_in,
+            tx_fft_out,
+            FFTW_FORWARD);
+
+    fftw::plan ifft_plan(N,
+            ifft_in,
+            ifft_out,
+            FFTW_BACKWARD);
+
     // Do a quick copy, so as to free the mutex
     {
         std::lock_guard<std::mutex> lock(m_mutex);
@@ -80,37 +129,9 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
             return result;
         }
 
-        if (!fftw_init) {
-            rx_fft_in  = fftwf_alloc_complex(N);
-            memset(rx_fft_in, 0, N * sizeof(fftwf_complex));
-            rx_fft_out = fftwf_alloc_complex(N);
-            memset(rx_fft_out, 0, N * sizeof(fftwf_complex));
-            tx_fft_in  = fftwf_alloc_complex(N);
-            memset(tx_fft_in, 0, N * sizeof(fftwf_complex));
-            tx_fft_out = fftwf_alloc_complex(N);
-            memset(tx_fft_out, 0, N * sizeof(fftwf_complex));
-            ifft_in    = fftwf_alloc_complex(N);
-            memset(ifft_in, 0, N * sizeof(fftwf_complex));
-            ifft_out   = fftwf_alloc_complex(N);
-            memset(ifft_out, 0, N * sizeof(fftwf_complex));
-
-            rx_fft_plan = fftwf_plan_dft_1d(N,
-                    rx_fft_in, rx_fft_out,
-                    FFTW_FORWARD, FFTW_MEASURE);
-
-            tx_fft_plan = fftwf_plan_dft_1d(N,
-                    tx_fft_in, tx_fft_out,
-                    FFTW_FORWARD, FFTW_MEASURE);
-
-            ifft_plan = fftwf_plan_dft_1d(N,
-                    ifft_in, ifft_out,
-                    FFTW_BACKWARD, FFTW_MEASURE);
-
-            fftw_init = true;
-        }
-
-        memcpy(rx_fft_in, &m_rxsamples.front(), len * sizeof(fftwf_complex));
-        memcpy(tx_fft_in, &m_txsamples.front(), len * sizeof(fftwf_complex));
+        std::copy(m_rxsamples.begin(), m_rxsamples.begin() + len, rx_fft_in.begin());
+        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);
@@ -125,16 +146,12 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
 
     // Calculate power
     for (size_t i = 0; i < len; i++) {
-        // Calculate norm
-        result.rx_power += rx_fft_in[i][0] * rx_fft_in[i][0] +
-                           rx_fft_in[i][1] * rx_fft_in[i][1];
+        result.rx_power += std::norm(rx_fft_in[i]);
     }
     result.rx_power = std::sqrt(result.rx_power);
 
     for (size_t i = 0; i < len; i++) {
-        // Calculate norm
-        result.tx_power += tx_fft_in[i][0] * tx_fft_in[i][0] +
-                           tx_fft_in[i][1] * tx_fft_in[i][1];
+        result.tx_power += std::norm(tx_fft_in[i]);
     }
     result.tx_power = std::sqrt(result.tx_power);
 
@@ -142,23 +159,17 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
     // corr(a, b) = ifft(fft(a) * conj(fft(b)))
     // Attention: circular correlation !
 
-    fftwf_execute(rx_fft_plan);
-
-    fftwf_execute(tx_fft_plan);
-
-    auto rx_fft = reinterpret_cast<std::complex<float>*>(rx_fft_out);
-    auto tx_fft = reinterpret_cast<std::complex<float>*>(tx_fft_out);
-    auto ifft   = reinterpret_cast<std::complex<float>*>(ifft_in);
+    rx_fft_plan.execute();
+    tx_fft_plan.execute();
 
     for (size_t i = 0; i < len; i++) {
-        ifft[i] = rx_fft[i] * std::conj(tx_fft[i]);
+        ifft_in[i] = rx_fft_out[i] * std::conj(tx_fft_out[i]);
     }
 
-    fftwf_execute(ifft_plan);
+    ifft_plan.execute();
 
-    auto ifft_out2 = reinterpret_cast<std::complex<float>*>(ifft_out);
     for (size_t i = 0; i < len; i++) {
-        result.correlation[i] = ifft_out2[i];
+        result.correlation[i] = ifft_out[i];
     }
 
 #if 0
diff --git a/AlignSample.hpp b/AlignSample.hpp
index 6bfdd6b..20bd2c8 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -56,7 +56,6 @@ class AlignSample {
             m_first_tx_sample_time = 0;
             m_num_rx_samples_dropped = 0;
             m_num_tx_samples_dropped = 0;
-            fftw_init = false;
 
             fd_rx = fopen("rx.debug", "wb");
             fd_tx = fopen("tx.debug", "wb");
@@ -104,17 +103,6 @@ class AlignSample {
 
         FILE* fd_rx;
         FILE* fd_tx;
-
-        bool fftw_init;
-        fftwf_complex *rx_fft_in;
-        fftwf_complex *rx_fft_out;
-        fftwf_complex *tx_fft_in;
-        fftwf_complex *tx_fft_out;
-        fftwf_complex *ifft_in;
-        fftwf_complex *ifft_out;
-        fftwf_plan rx_fft_plan;
-        fftwf_plan tx_fft_plan;
-        fftwf_plan ifft_plan;
 };
 
 
diff --git a/main.cpp b/main.cpp
index 3a1387d..cd3d96c 100644
--- a/main.cpp
+++ b/main.cpp
@@ -84,7 +84,7 @@ size_t do_receive(OutputUHD* output_uhd)
 
 void find_peak_correlation()
 {
-    FILE* fd = fopen("correlation.debug", "w");
+    FILE* fd = nullptr; //fopen("correlation.debug", "w");
 
     while (running) {
         const size_t correlation_length = 16 * 1024; // 8ms at 2048000
@@ -95,34 +95,34 @@ void find_peak_correlation()
             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",
-                    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);
             // Find correlation peak
             for (size_t offset = 0; offset < xcs.size(); offset++) {
                 complexf xc = xcs[offset];
-                fprintf(fd, "%f ", std::norm(xc));
+                if (fd) {
+                    fprintf(fd, "%f ", std::norm(xc));
+                }
                 if (std::norm(xc) >= max_norm) {
                     max_norm = std::norm(xc);
                     pos_max = offset;
                 }
             }
-            fprintf(fd, "\n");
-            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);
+            char msg[512];
+            snprintf(msg, 512, "Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
+                        std::sqrt(max_norm),
+                        (double)pos_max / (double)samplerate * 1000.0,
+                        pos_max,
+                        10*std::log(result.rx_power),
+                        10*std::log(result.tx_power),
+                        result.rx_timestamp,
+                        result.tx_timestamp);
+            if (fd) {
+                fprintf(fd, "\n%s", msg);
+            }
+            std::cerr << msg;
             std::this_thread::sleep_for(std::chrono::microseconds(1));
 
             // Eat much more than we correlate, because correlation is slow
-            aligner.consume(2048000);
+            aligner.consume(204800);
         }
         else {
             MDEBUG("Waiting for correlation\n");
@@ -165,43 +165,60 @@ int main(int argc, char **argv)
 
     FILE* fd = nullptr;
     if (uri == "test") {
-        FILE* fd_rx = fopen("rx.debug", "r");
-        FILE* fd_tx = fopen("tx.debug", "r");
+        FILE* fd_rx = fopen("rx.test", "r");
+        if (!fd_rx) {
+            std::cerr << "fx_rx open error" << std::endl;
+            abort();
+        }
+        FILE* fd_tx = fopen("tx.test", "r");
+        if (!fd_tx) {
+            std::cerr << "fx_tx open error" << std::endl;
+            abort();
+        }
 
-        const size_t len = 64;
+        size_t num_rx_samples;
+        size_t num_tx_samples;
+        do {
+            const size_t len = 64;
+            std::vector<complexf> rx_samples(len);
+            std::vector<complexf> tx_samples(len);
 
-        std::vector<complexf> rx_samples(len*1024);
-        std::vector<complexf> tx_samples(len*1024);
+            num_rx_samples = fread(&rx_samples.front(), sizeof(complexf), len, fd_rx);
+            num_tx_samples = fread(&tx_samples.front(), sizeof(complexf), len, fd_tx);
 
-        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.push_rx_samples(&rx_samples.front(), num_rx_samples, 1);
-        aligner.push_tx_samples(&tx_samples.front(), num_tx_samples, 1);
+            std::cerr << ".";
+        } while (num_rx_samples and num_tx_samples);
+        std::cerr << std::endl;
 
         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;
+        while (aligner.ready(correlation_length)) {
+            auto result = aligner.crosscorrelate(correlation_length);
+            auto& xcs = result.correlation;
+            for (size_t offset = 0; offset < xcs.size(); offset++) {
+                complexf& xc = xcs[offset];
+                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);
+            MDEBUG("Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
+                    std::sqrt(max_norm),
+                    (double)pos_max / (double)samplerate * 1000.0,
+                    pos_max,
+                    10*std::log(result.rx_power),
+                    10*std::log(result.tx_power),
+                    result.rx_timestamp,
+                    result.tx_timestamp);
 
+            aligner.consume(correlation_length / 2);
+        }
         return 0;
     }
     else if (uri.find("tcp://") != 0) {