Add subsample delay compensation (untested)

4 files changed, 151 insertions, 44 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index 4f5a8eb..dd29f64 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -1,5 +1,5 @@
 /*
-   Copyright (C) 2015
+   Copyright (C) 2017
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -23,6 +23,10 @@
 
 #include "AlignSample.hpp"
 #include <cstring>
+#include <exception>
+#include <algorithm>
+#include <numeric>
+#include <boost/math/tools/minima.hpp>
 
 namespace fftw {
     class plan {
@@ -119,12 +123,12 @@ 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);
+    vec_cf rx_fft_in(N);
+    vec_cf rx_fft_out(N);
+    vec_cf tx_fft_in(N);
+    vec_cf tx_fft_out(N);
+    vec_cf ifft_in(N);
+    vec_cf ifft_out(N);
 
     fftw::plan rx_fft_plan(N,
             rx_fft_in,
@@ -198,21 +202,113 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
     return result;
 }
 
-std::pair<std::vector<complexf>, std::vector<complexf> > AlignSample::get_samples(
-                size_t len, size_t rx_delay)
+void AlignSample::delay_rx_samples(size_t delay)
 {
-    std::pair<std::vector<complexf>, std::vector<complexf> > rval;
+    std::lock_guard<std::mutex> lock(m_mutex);
+    if (align() and delay > 0 and m_rxsamples.size() > delay) {
+        m_rxsamples.erase(
+                m_rxsamples.begin(),
+                m_rxsamples.begin() + delay - 1);
+    }
+}
+
+static std::vector<double> gen_omega(size_t length)
+{
+    if (not length % 2 == 0) {
+        throw std::runtime_error("Needs an even length array.");
+    }
+    size_t halflength = length/2;
+
+    double factor = 2.0 * M_PI / length;
+
+    std::vector<double> omega(length);
+    for (size_t i = 0; i < halflength; i++) {
+        omega[i] = factor * i;
+    }
+    for (size_t i = halflength+1; i < length; i++) {
+        omega[i] = factor * ((ssize_t)i - length);
+    }
+
+    return omega;
+}
+
+/* Find subsample delay in signal versus the reference signal ref and
+ * correct it
+ */
+static vec_cf align_subsample(vec_cf& signal, vec_cf& ref)
+{
+    size_t N = signal.size();
+    vec_cf fft_sig(N);
+    vec_cf rotate_vec(N);
+    vec_cf corr_sig(N);
+
+    fftw::plan plan(N, signal, fft_sig, FFTW_FORWARD);
+    fftw::plan plan_ifft(N, rotate_vec, corr_sig, FFTW_BACKWARD);
+    plan.execute();
+
+    const auto omega = gen_omega(N);
+    auto correlate_point = [&](float tau) {
+        for (size_t i = 0; i < N; i++) {
+            const complexf angle(0, tau * i);
+            rotate_vec[i] = std::exp(angle);
+        }
+        rotate_vec[N/2] = cos(M_PI * tau);
+
+        for (size_t i = 0; i < N; i++) {
+            rotate_vec[i] *= fft_sig[i];
+        }
+
+        plan_ifft.execute();
+
+        std::vector<float> corr_real(N);
+        std::transform(corr_sig.begin(), corr_sig.end(), corr_real.begin(),
+                [&](complexf f){ return f.real(); });
+
+        for (size_t i = 0; i < N; i++) {
+            complexf f = corr_real[i] * ref[i];
+            corr_real[i] = f.real();
+        }
+        return std::accumulate(corr_real.begin(), corr_real.end(), 0.0f);
+    };
+
+    const float start_arg = -1;
+    const float end_arg = 1;
+
+    auto tau =
+        boost::math::tools::brent_find_minima(
+                correlate_point, start_arg, end_arg, 1000).first;
+
+    fprintf(stderr, "Fractional delay is %f\n", tau);
+
+    for (size_t i = 0; i < N; i++) {
+        const complexf angle(0, tau * i);
+        rotate_vec[i] = std::exp(angle);
+    }
+    rotate_vec[N/2] = cos(M_PI * tau);
+    for (size_t i = 0; i < N; i++) {
+        rotate_vec[i] *= fft_sig[i];
+    }
+
+    plan_ifft.execute();
+
+    return corr_sig;
+}
+
+std::pair<vec_cf, vec_cf> AlignSample::get_samples(
+                size_t len)
+{
+    std::pair<vec_cf, vec_cf> rval;
 
     std::lock_guard<std::mutex> lock(m_mutex);
     if (align() and
-            m_rxsamples.size() > len + rx_delay
-            and m_txsamples.size() > len) {
+            m_rxsamples.size() > len and
+            m_txsamples.size() > len) {
 
         rval.first.reserve(len);
         rval.second.reserve(len);
 
-        std::copy(m_rxsamples.begin() + rx_delay,
-                m_rxsamples.begin() + rx_delay + len,
+        std::copy(m_rxsamples.begin(),
+                m_rxsamples.begin() + len,
                 std::back_inserter(rval.first));
 
         std::copy(m_txsamples.begin(),
diff --git a/AlignSample.hpp b/AlignSample.hpp
index d027823..9529792 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -1,5 +1,5 @@
 /*
-   Copyright (C) 2015
+   Copyright (C) 2017
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -33,7 +33,8 @@
 #define FFT_TYPE fftwf_complex
 
 
-typedef std::complex<float> complexf;
+using complexf = std::complex<float>;
+using vec_cf = std::vector<complexf>;
 
 struct CorrelationResult {
     CorrelationResult(size_t len) :
@@ -43,7 +44,7 @@ struct CorrelationResult {
         rx_timestamp(0),
         tx_timestamp(0) {}
 
-    std::vector<complexf> correlation;
+    vec_cf correlation;
     float rx_power;
     float tx_power;
 
@@ -85,8 +86,14 @@ class AlignSample {
 
         CorrelationResult crosscorrelate(size_t len);
 
-        std::pair<std::vector<complexf>, std::vector<complexf> > get_samples(
-                size_t len, size_t rx_delay);
+        /* Drop delay samples from the rx_samples */
+        void delay_rx_samples(size_t delay);
+
+        /* Calculate and compensate for subsample delay, assuming that the
+         * RX and TX samples are already aligned
+         */
+        std::pair<vec_cf, vec_cf> get_samples(
+                size_t len);
 
         void consume(size_t samples);
 
diff --git a/align/GenerateExampleTxRxIQ.py b/align/GenerateExampleTxRxIQ.py
index fc105fd..03c79b6 100755
--- a/align/GenerateExampleTxRxIQ.py
+++ b/align/GenerateExampleTxRxIQ.py
@@ -30,19 +30,6 @@ import scipy
 import scipy.signal
 import numpy as np
 
-## Configuration
-# whether to correct for delays larger than one sample
-# Not necessary unless you have delay larger than oversample/2
-do_integer_compensation = 0
-
-# by how much to oversample the signal before applying the delay
-oversample = 8
-
-# Add a delay of delay/oversample samples to the input signal
-#delay = 7
-
-do_plot = False
-
 def gen_omega(length):
     if not length % 2 == 0:
         raise ValueError('Needs an even length array.')
@@ -53,8 +40,9 @@ def gen_omega(length):
     omega[halflength+1:] = 2*np.pi*( np.arange(halflength+1, length)-length )
     return omega / length
 
-def gen_signals(delay):
-    ## Generate signal
+def gen_signals(oversample, delay):
+    """Generate a signal that is delayed and a bit noisy. Returns a tuple
+    (original signal, shifted signal)"""
 
     iq_file = "/home/bram/dab/aux/odr-dab-cir/phasereference.2048000.fc64.iq"
 
@@ -137,13 +125,28 @@ def delay_signal(sig, delay):
 
     return np.fft.ifft(sig_fft * rotate_vec)
 
-for d in [2, 5, 9, 15, 34, 120]:
-    a, b = gen_signals(d)
-    delay = arg_max_corr(a,b)
-    print("{} {}".format(d / oversample, delay))
+def fftplot(sig):
+    plt.plot(np.abs(np.fft.fftshift(np.fft.fft(sig))))
+
+if __name__ == '__main__':
+    # Add a delay of d/oversample samples to the input signal
+
+    # by how much to oversample the signal before applying the delay
+    oversample = 8
+
+    do_plot = False
+
+    for d in [2, 7]:
+        a, b = gen_signals(d, oversample)
+        delay = arg_max_corr(a,b)
+        print("{} {}".format(d / oversample, delay))
 
-    print("{} {}".format(d / oversample, arg_max_corr(a, delay_signal(b, delay))))
+        print("{} {}".format(d / oversample, arg_max_corr(a, delay_signal(b, delay))))
 
+        if do_plot:
+            plt.figure()
+            fftplot(a)
+            fftplot(a-b)
 
-if do_plot:
-    plt.show()
+    if do_plot:
+        plt.show()
diff --git a/main.cpp b/main.cpp
index b5dcf2f..88eb846 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,5 +1,5 @@
 /*
-   Copyright (C) 2015
+   Copyright (C) 2017
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -62,7 +62,7 @@ AlignSample aligner;
 
 PointCloud cloud(10000);
 
-size_t do_receive(OutputUHD* output_uhd)
+size_t do_receive(OutputUHD *output_uhd)
 {
     std::vector<complexf> samps(samps_per_buffer);
     double first_sample_time = 0;
@@ -99,7 +99,8 @@ long user_delay = 0;
 
 void push_to_point_cloud(size_t rx_delay)
 {
-    auto points = aligner.get_samples(correlation_length, rx_delay + user_delay);
+    aligner.delay_rx_samples(rx_delay + user_delay);
+    auto points = aligner.get_samples(correlation_length);
 
     if (points.first.size() > 0) {
         cloud.push_samples(points);