diff options
Diffstat (limited to 'AlignSample.cpp')
| -rw-r--r-- | AlignSample.cpp | 124 |
1 files changed, 110 insertions, 14 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(), |