Replace manual correlation by FFT

5 files changed, 185 insertions, 58 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index d2d9339..2fcb830 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -22,9 +22,14 @@
  */
 
 #include "AlignSample.hpp"
+#include <cstring>
 
 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) {
+        fwrite(samps, sizeof(complexf), len, fd_tx);
+    }
+
     std::lock_guard<std::mutex> lock(m_mutex);
     std::copy(samps, samps + len, std::back_inserter(m_txsamples));
 
@@ -35,6 +40,10 @@ void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_samp
 
 void AlignSample::push_rx_samples(complexf* samps, size_t len, double first_sample_time)
 {
+    if (first_sample_time > 5.1 and first_sample_time < 5.5) {
+        fwrite(samps, sizeof(complexf), len, fd_rx);
+    }
+
     std::lock_guard<std::mutex> lock(m_mutex);
     std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
 
@@ -49,60 +58,120 @@ bool AlignSample::ready(size_t min_samples)
     return align() and m_rxsamples.size() > min_samples and m_txsamples.size() > min_samples;
 }
 
-CorrelationResult AlignSample::crosscorrelate(size_t max_offset, size_t len)
+CorrelationResult AlignSample::crosscorrelate(size_t len)
 {
-    std::vector<complexf> rxsamps;
-    std::vector<complexf> txsamps;
     double rx_ts = 0;
     double tx_ts = 0;
 
+    /* The size of the FFT is twice as long as the desired
+     * correlation length, because the FFT does a circular
+     * correlation, and we therefore pad half with zeros
+     */
+    const size_t N = 2 * len;
+
     // Do a quick copy, so as to free the mutex
     {
         std::lock_guard<std::mutex> lock(m_mutex);
 
         if (!align() or
                 m_rxsamples.size() < len or
-                m_txsamples.size() < len + max_offset) {
+                m_txsamples.size() < len) {
             CorrelationResult result(0);
             return result;
         }
 
-        std::copy(m_rxsamples.begin(), m_rxsamples.begin() + len, std::back_inserter(rxsamps));
-        std::copy(m_txsamples.begin(), m_txsamples.begin() + len + max_offset, std::back_inserter(txsamps));
+        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));
 
         m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + len);
-        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len + max_offset);
+        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len);
 
         rx_ts = m_rx_sample_time();
         tx_ts = m_tx_sample_time();
     }
 
-    CorrelationResult result(max_offset);
+    CorrelationResult result(len);
     result.rx_timestamp = rx_ts;
     result.tx_timestamp = tx_ts;
 
-    auto& xcorrs = result.correlation;
-
     // Calculate power
-    for (auto sample : rxsamps) {
-        result.rx_power += std::norm(sample);
+    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::sqrt(result.rx_power);
 
-    for (auto sample : txsamps) {
-        result.tx_power += std::norm(sample);
+    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::sqrt(result.tx_power);
 
+    // Implement
+    // 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);
+
+    for (size_t i = 0; i < len; i++) {
+        ifft[i] = rx_fft[i] * std::conj(tx_fft[i]);
+    }
+
+    fftwf_execute(ifft_plan);
+
+    auto ifft_out2 = reinterpret_cast<std::complex<float>*>(ifft_out);
+    for (size_t i = 0; i < len; i++) {
+        result.correlation[i] = ifft_out2[i];
+    }
+
+#if 0
     // Calculate correlation
     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]);
+            xcorr += rxsamps[i]/rx_power_f * std::conj(txsamps[i+offset])/tx_power_f;
         }
-        xcorrs[offset] = xcorr;
+        result.correlation[offset] = xcorr;
     }
+#endif
 
     return result;
 }
diff --git a/AlignSample.hpp b/AlignSample.hpp
index 31b5de2..6bfdd6b 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -27,6 +27,9 @@
 #include <deque>
 #include <mutex>
 #include <complex>
+#include "fftw3.h"
+#define FFT_TYPE fftwf_complex
+
 
 typedef std::complex<float> complexf;
 
@@ -39,8 +42,8 @@ struct CorrelationResult {
         tx_timestamp(0) {}
 
     std::vector<complexf> correlation;
-    double rx_power;
-    double tx_power;
+    float rx_power;
+    float tx_power;
 
     double rx_timestamp;
     double tx_timestamp;
@@ -53,6 +56,11 @@ 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");
+
         }
 
         void push_tx_samples(complexf* samps, size_t len, double first_sample_time);
@@ -68,7 +76,7 @@ class AlignSample {
             MDEBUG("  TX: %f %zu\n", m_tx_sample_time(), m_txsamples.size());
         }
 
-        CorrelationResult crosscorrelate(size_t max_offset, size_t len);
+        CorrelationResult crosscorrelate(size_t len);
 
         void consume(size_t samples);
 
@@ -93,6 +101,20 @@ class AlignSample {
         double m_first_tx_sample_time;
         size_t m_num_tx_samples_dropped;
         std::deque<complexf> m_txsamples;
+
+        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/CMakeLists.txt b/CMakeLists.txt
index 891b853..59067e1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,6 +51,7 @@ add_definitions(-Wall)
 find_package(PkgConfig)
 pkg_check_modules(UHD uhd)
 pkg_check_modules(ZMQ libzmq)
+pkg_check_modules(FFTW3F fftw3f)
 # Threads
 find_package(Threads REQUIRED)
 
@@ -65,7 +66,11 @@ list(APPEND odrdpd_sources
     AlignSample.cpp
     )
 
-list(APPEND common_link_list ${UHD_LIBRARIES} ${ZMQ_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
+list(APPEND common_link_list
+    ${UHD_LIBRARIES}
+    ${ZMQ_LIBRARIES}
+    ${FFTW3F_LIBRARIES}
+    ${CMAKE_THREAD_LIBS_INIT})
 
 set_source_files_properties(
     ${odrdpd_sources}
diff --git a/OutputUHD.cpp b/OutputUHD.cpp
index fff3e15..6800daf 100644
--- a/OutputUHD.cpp
+++ b/OutputUHD.cpp
@@ -52,8 +52,12 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) :
     m_usrp->set_rx_rate(m_samplerate);
 
     m_usrp->set_tx_freq(234.208e6);
-    double set_frequency = m_usrp->get_tx_freq();
-    MDEBUG("OutputUHD:Actual frequency: %f\n", set_frequency);
+    double set_tx_frequency = m_usrp->get_tx_freq();
+    MDEBUG("OutputUHD:Actual TX frequency: %f\n", set_tx_frequency);
+
+    m_usrp->set_rx_freq(234.208e6);
+    double set_rx_frequency = m_usrp->get_rx_freq();
+    MDEBUG("OutputUHD:Actual RX frequency: %f\n", set_rx_frequency);
 
     MDEBUG("OutputUHD:Setting TX Gain: %f ...\n", m_txgain);
     m_usrp->set_tx_gain(m_txgain);
@@ -76,10 +80,8 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) :
     myTxStream = m_usrp->get_tx_stream(stream_args);
 
     myRxStream = m_usrp->get_rx_stream(stream_args);
-    uhd::stream_cmd_t stream_cmd(true ?
-        uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS:
-        uhd::stream_cmd_t::STREAM_MODE_NUM_SAMPS_AND_DONE
-    );
+    uhd::stream_cmd_t stream_cmd(
+        uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
     stream_cmd.num_samps = 0;
     stream_cmd.stream_now = true;
     stream_cmd.time_spec = uhd::time_spec_t();
@@ -115,33 +117,23 @@ size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double *first
 size_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time)
 {
     const double rx_timeout = 20.0;
+    const size_t usrp_max_num_samps = myRxStream->get_max_num_samps();
+    const size_t samps_to_rx = std::min(sizeIn, usrp_max_num_samps);
 
     uhd::rx_metadata_t md;
-    size_t usrp_max_num_samps = myRxStream->get_max_num_samps();
-
-    *first_sample_time = md.time_spec.get_real_secs();
+    size_t num_rx_samps = myRxStream->recv(samples, samps_to_rx, md, rx_timeout);
 
-    size_t num_acc_samps = 0; //number of accumulated samples
-    while (num_acc_samps < sizeIn) {
-        size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);
+    if (md.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE){
+        MDEBUG("RX Error %s\n", md.strerror().c_str());
+    }
 
-        //send a single packet
-        size_t num_rx_samps = myRxStream->recv(
-                &samples[num_acc_samps],
-                samps_to_send, md, rx_timeout);
-
-        if (num_acc_samps == 0) {
-            if (md.has_time_spec) {
-                *first_sample_time = md.time_spec.get_real_secs();
-            }
-            else {
-                MDEBUG("samp %zu no time spec!\n", num_acc_samps);
-            }
-        }
-
-        num_acc_samps += num_rx_samps;
+    if (md.has_time_spec) {
+        *first_sample_time = md.time_spec.get_real_secs();
+    }
+    else {
+        *first_sample_time = 0;
     }
 
-    return num_acc_samps;
+    return num_rx_samps;
 }
 
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);
         }