Do not count time in double, save correlator debug

3 files changed, 81 insertions, 42 deletions

diff --git a/OutputUHD.cpp b/OutputUHD.cpp
index 71ecde0..fff3e15 100644
--- a/OutputUHD.cpp
+++ b/OutputUHD.cpp
@@ -37,11 +37,11 @@
 
 using namespace std;
 
-OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate)
+OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) :
+    m_txgain(txgain),
+    m_rxgain(rxgain),
+    m_samplerate(samplerate)
 {
-    m_txgain = txgain;
-    m_samplerate = samplerate;
-
     uhd::set_thread_priority_safe();
 
     string device = "master_clock_rate=32768000";
@@ -55,9 +55,11 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate)
     double set_frequency = m_usrp->get_tx_freq();
     MDEBUG("OutputUHD:Actual frequency: %f\n", set_frequency);
 
+    MDEBUG("OutputUHD:Setting TX Gain: %f ...\n", m_txgain);
     m_usrp->set_tx_gain(m_txgain);
     MDEBUG("OutputUHD:Actual TX Gain: %f ...\n", m_usrp->get_tx_gain());
 
+    MDEBUG("OutputUHD:Setting RX Gain: %f ...\n", m_rxgain);
     m_usrp->set_rx_gain(m_rxgain);
     MDEBUG("OutputUHD:Actual RX Gain: %f ...\n", m_usrp->get_rx_gain());
 
diff --git a/OutputUHD.hpp b/OutputUHD.hpp
index 6dc20dd..37d8397 100644
--- a/OutputUHD.hpp
+++ b/OutputUHD.hpp
@@ -37,9 +37,9 @@ class OutputUHD {
         size_t Receive(complexf *samples, size_t sizeIn, double *first_sample_time);
 
     private:
-        double m_samplerate;
         double m_txgain;
         double m_rxgain;
+        double m_samplerate;
         uhd::usrp::multi_usrp::sptr m_usrp;
         uhd::tx_metadata_t md;
         uhd::tx_streamer::sptr myTxStream;
diff --git a/main.cpp b/main.cpp
index f91f6a5..88edf6d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -62,26 +62,33 @@ struct CorrelationResult {
     CorrelationResult(size_t len) :
         correlation(len),
         rx_power(0),
-        tx_power(0) {}
+        tx_power(0),
+        rx_timestamp(0),
+        tx_timestamp(0) {}
 
     std::vector<complexf> correlation;
     double rx_power;
     double tx_power;
+
+    double rx_timestamp;
+    double tx_timestamp;
 };
 
 class AlignSample {
     public:
         AlignSample() {
-            m_rx_sample_time = 0;
-            m_tx_sample_time = 0;
+            m_first_rx_sample_time = 0;
+            m_first_tx_sample_time = 0;
+            m_num_rx_samples_dropped = 0;
+            m_num_tx_samples_dropped = 0;
         }
 
         void 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_tx_sample_time == 0) {
-                m_tx_sample_time = first_sample_time;
+            if (m_first_tx_sample_time == 0) {
+                m_first_tx_sample_time = first_sample_time;
             }
         }
 
@@ -89,41 +96,53 @@ class AlignSample {
             std::lock_guard<std::mutex> lock(m_mutex);
             std::copy(samps, samps + len, std::back_inserter(m_rxsamples));
 
-            if (m_rx_sample_time == 0) {
-                m_rx_sample_time = first_sample_time;
+            if (m_first_rx_sample_time == 0) {
+                m_first_rx_sample_time = first_sample_time;
             }
         }
 
-        bool ready() {
+        bool ready(size_t min_samples) {
             std::lock_guard<std::mutex> lock(m_mutex);
-            return aligned() and m_rxsamples.size() > 8000 and m_txsamples.size() > 8000;
+            return align() and m_rxsamples.size() > min_samples and m_txsamples.size() > min_samples;
         }
 
         void debug() {
+            std::lock_guard<std::mutex> lock(m_mutex);
             MDEBUG("Aligner\n");
-            MDEBUG("  RX: %f %zu\n", m_rx_sample_time, m_rxsamples.size());
-            MDEBUG("  TX: %f %zu\n", m_tx_sample_time, m_txsamples.size());
+            MDEBUG("  RX: %f %zu\n", m_rx_sample_time(), m_rxsamples.size());
+            MDEBUG("  TX: %f %zu\n", m_tx_sample_time(), m_txsamples.size());
         }
 
         CorrelationResult 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 (m_rxsamples.size() < len or
-                    m_txsamples.size() < len + max_offset) {
+                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;
 
@@ -154,50 +173,60 @@ class AlignSample {
         void consume(size_t samples)
         {
             std::lock_guard<std::mutex> lock(m_mutex);
-            if (aligned() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
+            if (align() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
                 m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
-                m_rx_sample_time += (double)samples / samplerate;
+                m_num_rx_samples_dropped += samples;
 
                 m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
-                m_tx_sample_time += (double)samples / samplerate;
+                m_num_tx_samples_dropped += samples;
             }
         }
 
     private:
-        bool aligned() {
-            if (std::abs(m_rx_sample_time - m_tx_sample_time) < 1e-6) {
+        bool 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;
+            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_rx_sample_time += (double)rx_samples_to_skip / samplerate;
+                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;
+            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_tx_sample_time += (double)tx_samples_to_skip / samplerate;
+                m_num_tx_samples_dropped += tx_samples_to_skip;
                 return true;
             }
             return false;
         }
 
+        double m_rx_sample_time() {
+            return m_first_rx_sample_time + (double)m_num_rx_samples_dropped / samplerate;
+        }
+
+        double m_tx_sample_time() {
+            return m_first_tx_sample_time + (double)m_num_tx_samples_dropped / samplerate;
+        }
+
         std::mutex m_mutex;
-        double m_rx_sample_time;
+        double m_first_rx_sample_time;
+        size_t m_num_rx_samples_dropped;
         std::deque<complexf> m_rxsamples;
 
-        double m_tx_sample_time;
+        double m_first_tx_sample_time;
+        size_t m_num_tx_samples_dropped;
         std::deque<complexf> m_txsamples;
 };
 
@@ -229,10 +258,12 @@ size_t do_receive(OutputUHD* output_uhd)
 
 void find_peak_correlation()
 {
+    FILE* fd = fopen("correlation.debug", "w");
+
     while (running) {
-        if (aligner.ready()) {
-            const size_t max_offset = 100000; // 48ms at 2048000
-            const size_t correlation_length = 100;
+        const size_t max_offset = 10000; // 4.8ms at 2048000
+        if (aligner.ready(max_offset)) {
+            const size_t correlation_length = 1000;
             std::vector<complexf> correlations(max_offset);
             double max_norm = 0.0;
             size_t pos_max = 0;
@@ -240,28 +271,37 @@ void find_peak_correlation()
             auto result = aligner.crosscorrelate(max_offset, correlation_length);
             auto& xcs = result.correlation;
 
+            fprintf(fd, "Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
+                    std::sqrt(max_norm),
+                    (double)pos_max / (double)samplerate * 1000.0,
+                    10*std::log(result.rx_power),
+                    10*std::log(result.tx_power),
+                    result.rx_timestamp,
+                    result.tx_timestamp);
             // Find correlation peak
             for (size_t offset = 0; offset < xcs.size(); offset++) {
                 complexf xc = xcs[offset];
+                fprintf(fd, "%f ", std::norm(xc));
                 if (std::norm(xc) >= max_norm) {
                     max_norm = std::norm(xc);
                     pos_max = offset;
                 }
             }
-            MDEBUG("Max correlation is %f at %zu, with RX %fdB and TX %fdB\n",
+            fprintf(fd, "\n");
+            MDEBUG("Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
                     std::sqrt(max_norm),
-                    pos_max,
+                    (double)pos_max / (double)samplerate * 1000.0,
                     10*std::log(result.rx_power),
-                    10*std::log(result.tx_power));
+                    10*std::log(result.tx_power),
+                    result.rx_timestamp,
+                    result.tx_timestamp);
             std::this_thread::sleep_for(std::chrono::microseconds(1));
 
             // Eat much more than we correlate, because correlation is slow
             aligner.consume(2048000);
-            aligner.debug();
         }
         else {
             MDEBUG("Waiting for correlation\n");
-            aligner.debug();
             std::this_thread::sleep_for(std::chrono::seconds(1));
         }
     }
@@ -289,9 +329,6 @@ int main(int argc, char **argv)
         }
     }
 
-    MDEBUG("TX Gain is %f\n", txgain);
-    MDEBUG("RX Gain is %f\n", rxgain);
-
     if (argc < 2) {
         MDEBUG("Require input file or url\n");
         return -1;