diff options
Diffstat (limited to 'AlignSample.cpp')
| -rw-r--r-- | AlignSample.cpp | 101 |
1 files changed, 85 insertions, 16 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; } |