aboutsummaryrefslogtreecommitdiffstats
path: root/AlignSample.cpp
diff options
context:
space:
mode:
authorMatthias P. Braendli <matthias.braendli@mpb.li>2015-11-08 18:32:48 +0100
committerMatthias P. Braendli <matthias.braendli@mpb.li>2015-11-08 18:32:48 +0100
commitf02555f2f6631b03663cb02da6659e926db07db8 (patch)
tree3ac43a8a4ee75d2bd61ca1ed108e4fa9590452ec /AlignSample.cpp
parent823043497a9fd59ac86e9596c56df8e692340974 (diff)
downloadodr-dpd-f02555f2f6631b03663cb02da6659e926db07db8.tar.gz
odr-dpd-f02555f2f6631b03663cb02da6659e926db07db8.tar.bz2
odr-dpd-f02555f2f6631b03663cb02da6659e926db07db8.zip
Replace manual correlation by FFT
Diffstat (limited to 'AlignSample.cpp')
-rw-r--r--AlignSample.cpp101
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;
}