diff options
| -rw-r--r-- | AlignSample.cpp | 124 | ||||
| -rw-r--r-- | AlignSample.hpp | 17 | ||||
| -rwxr-xr-x | align/GenerateExampleTxRxIQ.py | 47 | ||||
| -rw-r--r-- | main.cpp | 7 |
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() @@ -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); |