Calculate gain difference

1 files changed, 42 insertions, 9 deletions

diff --git a/main.cpp b/main.cpp
index 3e83074..f91f6a5 100644
--- a/main.cpp
+++ b/main.cpp
@@ -58,6 +58,17 @@ size_t read_samples(FILE* fd, std::vector<complexf>& samples, size_t count)
     return num_read;
 }
 
+struct CorrelationResult {
+    CorrelationResult(size_t len) :
+        correlation(len),
+        rx_power(0),
+        tx_power(0) {}
+
+    std::vector<complexf> correlation;
+    double rx_power;
+    double tx_power;
+};
+
 class AlignSample {
     public:
         AlignSample() {
@@ -94,7 +105,7 @@ class AlignSample {
             MDEBUG("  TX: %f %zu\n", m_tx_sample_time, m_txsamples.size());
         }
 
-        std::vector<complexf> crosscorrelate(size_t max_offset, size_t len) {
+        CorrelationResult crosscorrelate(size_t max_offset, size_t len) {
             std::vector<complexf> rxsamps;
             std::vector<complexf> txsamps;
 
@@ -104,15 +115,30 @@ class AlignSample {
 
                 if (m_rxsamples.size() < len or
                     m_txsamples.size() < len + max_offset) {
-                    return {};
+                    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));
             }
 
-            std::vector<complexf> xcorrs(max_offset);
+            CorrelationResult result(max_offset);
+
+            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);
 
@@ -121,7 +147,8 @@ class AlignSample {
                 }
                 xcorrs[offset] = xcorr;
             }
-            return xcorrs;
+
+            return result;
         }
 
         void consume(size_t samples)
@@ -207,19 +234,25 @@ void find_peak_correlation()
             const size_t max_offset = 100000; // 48ms at 2048000
             const size_t correlation_length = 100;
             std::vector<complexf> correlations(max_offset);
-            double max_ampl = 0.0;
+            double max_norm = 0.0;
             size_t pos_max = 0;
 
-            auto xcs = aligner.crosscorrelate(max_offset, correlation_length);
+            auto result = aligner.crosscorrelate(max_offset, correlation_length);
+            auto& xcs = result.correlation;
 
+            // Find correlation peak
             for (size_t offset = 0; offset < xcs.size(); offset++) {
                 complexf xc = xcs[offset];
-                if (std::abs(xc) >= max_ampl) {
-                    max_ampl = std::abs(xc);
+                if (std::norm(xc) >= max_norm) {
+                    max_norm = std::norm(xc);
                     pos_max = offset;
                 }
             }
-            MDEBUG("Max correlation is %f at %zu\n", max_ampl, pos_max);
+            MDEBUG("Max correlation is %f at %zu, with RX %fdB and TX %fdB\n",
+                    std::sqrt(max_norm),
+                    pos_max,
+                    10*std::log(result.rx_power),
+                    10*std::log(result.tx_power));
             std::this_thread::sleep_for(std::chrono::microseconds(1));
 
             // Eat much more than we correlate, because correlation is slow