From c5343f4c9970fd4edab0e49dba841d6db341c042 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Wed, 16 Aug 2017 16:09:35 +0200
Subject: [PATCH] UDPSink buffer: implemented R/W pointer skew auto
 compensation optimization with average pulling

---
 plugins/channeltx/udpsink/udpsink.cpp         | 37 ++++++++++++++++++-
 plugins/channeltx/udpsink/udpsink.h           |  4 ++
 plugins/channeltx/udpsink/udpsinkmsg.h        | 11 ++++--
 .../channeltx/udpsink/udpsinkudphandler.cpp   |  2 +-
 4 files changed, 48 insertions(+), 6 deletions(-)

diff --git a/plugins/channeltx/udpsink/udpsink.cpp b/plugins/channeltx/udpsink/udpsink.cpp
index 31845ceaa..0a63fc608 100644
--- a/plugins/channeltx/udpsink/udpsink.cpp
+++ b/plugins/channeltx/udpsink/udpsink.cpp
@@ -29,6 +29,8 @@ UDPSink::UDPSink(MessageQueue* uiMessageQueue, UDPSinkGUI* udpSinkGUI, BasebandS
     m_spectrum(spectrum),
     m_magsq(1e-10),
     m_movingAverage(16, 0),
+    m_sampleRateSum(0),
+    m_sampleRateAvgCounter(0),
     m_settingsMutex(QMutex::Recursive)
 {
     setObjectName("UDPSink");
@@ -167,7 +169,38 @@ bool UDPSink::handleMessage(const Message& cmd)
     {
         UDPSinkMessages::MsgSampleRateCorrection& cfg = (UDPSinkMessages::MsgSampleRateCorrection&) cmd;
         m_actualInputSampleRate += cfg.getCorrectionFactor() * m_actualInputSampleRate;
-        qDebug("UDPSink::handleMessage: MsgSampleRateCorrection: corr: %f new rate: %f", cfg.getCorrectionFactor(), m_actualInputSampleRate);
+
+        if ((cfg.getRawDeltaRatio() > -0.05) || (cfg.getRawDeltaRatio() < 0.05))
+        {
+            if (m_sampleRateAvgCounter < m_sampleRateAverageItems)
+            {
+                m_sampleRateSum += m_actualInputSampleRate;
+                m_sampleRateAvgCounter++;
+            }
+        }
+        else
+        {
+            m_sampleRateSum = 0.0;
+            m_sampleRateAvgCounter = 0;
+        }
+
+        if (m_sampleRateAvgCounter == m_sampleRateAverageItems)
+        {
+            float avgRate = m_sampleRateSum / m_sampleRateAverageItems;
+            qDebug("UDPSink::handleMessage: MsgSampleRateCorrection: corr: %f new rate: %f: avg rate: %f",
+                    cfg.getCorrectionFactor(),
+                    m_actualInputSampleRate,
+                    avgRate);
+            m_actualInputSampleRate = avgRate;
+            m_sampleRateSum = 0.0;
+            m_sampleRateAvgCounter = 0;
+        }
+        else
+        {
+            qDebug("UDPSink::handleMessage: MsgSampleRateCorrection: corr: %f new rate: %f",
+                    cfg.getCorrectionFactor(),
+                    m_actualInputSampleRate);
+        }
 
         m_settingsMutex.lock();
         m_interpolatorDistanceRemain = 0;
@@ -233,6 +266,8 @@ void UDPSink::apply(bool force)
         m_interpolator.create(48, m_config.m_inputSampleRate, m_config.m_rfBandwidth / 2.2, 3.0);
         m_actualInputSampleRate = m_config.m_inputSampleRate;
         m_udpHandler.resetReadIndex();
+        m_sampleRateSum = 0.0;
+        m_sampleRateAvgCounter = 0;
         m_settingsMutex.unlock();
     }
 
diff --git a/plugins/channeltx/udpsink/udpsink.h b/plugins/channeltx/udpsink/udpsink.h
index 8ea12c53a..7345f5846 100644
--- a/plugins/channeltx/udpsink/udpsink.h
+++ b/plugins/channeltx/udpsink/udpsink.h
@@ -199,9 +199,13 @@ private:
 
     UDPSinkUDPHandler m_udpHandler;
     Real m_actualInputSampleRate; //!< sample rate with UDP buffer skew compensation
+    double m_sampleRateSum;
+    int m_sampleRateAvgCounter;
 
     QMutex m_settingsMutex;
 
+    static const int m_sampleRateAverageItems = 17;
+
     void apply(bool force);
     void modulateSample();
 };
diff --git a/plugins/channeltx/udpsink/udpsinkmsg.h b/plugins/channeltx/udpsink/udpsinkmsg.h
index 1843250a9..efe062461 100644
--- a/plugins/channeltx/udpsink/udpsinkmsg.h
+++ b/plugins/channeltx/udpsink/udpsinkmsg.h
@@ -30,18 +30,21 @@ public:
 
     public:
         float getCorrectionFactor() const { return m_correctionFactor; }
+        float getRawDeltaRatio() const { return m_rawDeltaRatio; }
 
-        static MsgSampleRateCorrection* create(float correctionFactor)
+        static MsgSampleRateCorrection* create(float correctionFactor, float rawDeltaRatio)
         {
-            return new MsgSampleRateCorrection(correctionFactor);
+            return new MsgSampleRateCorrection(correctionFactor, rawDeltaRatio);
         }
 
     private:
         float m_correctionFactor;
+        float m_rawDeltaRatio;
 
-        MsgSampleRateCorrection(float correctionFactor) :
+        MsgSampleRateCorrection(float correctionFactor, float rawDeltaRatio) :
             Message(),
-            m_correctionFactor(correctionFactor)
+            m_correctionFactor(correctionFactor),
+            m_rawDeltaRatio(rawDeltaRatio)
         { }
     };
 };
diff --git a/plugins/channeltx/udpsink/udpsinkudphandler.cpp b/plugins/channeltx/udpsink/udpsinkudphandler.cpp
index 216d58725..7091826ef 100644
--- a/plugins/channeltx/udpsink/udpsinkudphandler.cpp
+++ b/plugins/channeltx/udpsink/udpsinkudphandler.cpp
@@ -150,7 +150,7 @@ void UDPSinkUDPHandler::advanceReadPointer(int nbBytes)
                 float dd = d - m_d; // derivative
                 float c = (d / 15.0) + (dd / 20.0); // damping and scaling
                 c = c < -0.05 ? -0.05 : c > 0.05 ? 0.05 : c; // limit
-                UDPSinkMessages::MsgSampleRateCorrection *msg = UDPSinkMessages::MsgSampleRateCorrection::create(c);
+                UDPSinkMessages::MsgSampleRateCorrection *msg = UDPSinkMessages::MsgSampleRateCorrection::create(c, d);
 
                 if (m_feedbackMessageQueue) {
                     m_feedbackMessageQueue->push(msg);