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