Move AlignSample to separate file

1 files changed, 156 insertions, 0 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
new file mode 100644
index 0000000..d2d9339
--- /dev/null
+++ b/AlignSample.cpp
@@ -0,0 +1,156 @@
+/*
+   Copyright (C) 2015
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
+ */
+/*
+   This file is part of ODR-DPD.
+
+   ODR-DPD is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as
+   published by the Free Software Foundation, either version 3 of the
+   License, or (at your option) any later version.
+
+   ODR-DPD is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with ODR-DPD.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AlignSample.hpp"
+
+void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_sample_time)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    std::copy(samps, samps + len, std::back_inserter(m_txsamples));
+
+    if (m_first_tx_sample_time == 0) {
+        m_first_tx_sample_time = first_sample_time;
+    }
+}
+
+void AlignSample::push_rx_samples(complexf* samps, size_t len, double first_sample_time)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
+
+    if (m_first_rx_sample_time == 0) {
+        m_first_rx_sample_time = first_sample_time;
+    }
+}
+
+bool AlignSample::ready(size_t min_samples)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return align() and m_rxsamples.size() > min_samples and m_txsamples.size() > min_samples;
+}
+
+CorrelationResult AlignSample::crosscorrelate(size_t max_offset, size_t len)
+{
+    std::vector<complexf> rxsamps;
+    std::vector<complexf> txsamps;
+    double rx_ts = 0;
+    double tx_ts = 0;
+
+    // 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) {
+            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));
+
+        m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + len);
+        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len + max_offset);
+
+        rx_ts = m_rx_sample_time();
+        tx_ts = m_tx_sample_time();
+    }
+
+    CorrelationResult result(max_offset);
+    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);
+    }
+    result.rx_power = std::sqrt(result.rx_power);
+
+    for (auto sample : txsamps) {
+        result.tx_power += std::norm(sample);
+    }
+    result.tx_power = std::sqrt(result.tx_power);
+
+    // 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]);
+        }
+        xcorrs[offset] = xcorr;
+    }
+
+    return result;
+}
+
+void AlignSample::consume(size_t samples)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    if (align() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
+        m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
+        m_num_rx_samples_dropped += samples;
+
+        m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
+        m_num_tx_samples_dropped += samples;
+    }
+}
+
+bool AlignSample::align()
+{
+    if (std::abs(m_rx_sample_time() - m_tx_sample_time()) < 1e-6) {
+        return true;
+    }
+    else if (m_rx_sample_time() < m_tx_sample_time()) {
+        size_t rx_samples_to_skip =
+            (m_tx_sample_time() - m_rx_sample_time()) * samplerate;
+
+        if (rx_samples_to_skip > m_rxsamples.size()) {
+            return false;
+        }
+
+        m_rxsamples.erase(
+                m_rxsamples.begin(), m_rxsamples.begin() + rx_samples_to_skip);
+
+        m_num_rx_samples_dropped += rx_samples_to_skip;
+        return true;
+    }
+    else if (m_rx_sample_time() > m_tx_sample_time()) {
+        size_t tx_samples_to_skip =
+            (m_rx_sample_time() - m_tx_sample_time()) * samplerate;
+
+        if (tx_samples_to_skip > m_txsamples.size()) {
+            return false;
+        }
+
+        m_txsamples.erase(
+                m_txsamples.begin(), m_txsamples.begin() + tx_samples_to_skip);
+
+        m_num_tx_samples_dropped += tx_samples_to_skip;
+        return true;
+    }
+    return false;
+}