From 60b502ce7e9a312f896e5942f01787e11bcee5f2 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Wed, 16 Aug 2017 11:35:47 +0200
Subject: [PATCH] UDPSink buffer: implemented R/W pointer skew auto
 compensation

---
 plugins/channeltx/udpsink/udpsink.cpp         | 16 +++++++++---
 plugins/channeltx/udpsink/udpsink.h           |  1 +
 .../channeltx/udpsink/udpsinkudphandler.cpp   | 26 +++++++++++++++++--
 plugins/channeltx/udpsink/udpsinkudphandler.h |  3 ++-
 4 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/plugins/channeltx/udpsink/udpsink.cpp b/plugins/channeltx/udpsink/udpsink.cpp
index 62596b562..31845ceaa 100644
--- a/plugins/channeltx/udpsink/udpsink.cpp
+++ b/plugins/channeltx/udpsink/udpsink.cpp
@@ -115,8 +115,6 @@ void UDPSink::modulateSample()
 
 bool UDPSink::handleMessage(const Message& cmd)
 {
-    qDebug() << "UDPSink::handleMessage";
-
     if (UpChannelizer::MsgChannelizerNotification::match(cmd))
     {
         UpChannelizer::MsgChannelizerNotification& notif = (UpChannelizer::MsgChannelizerNotification&) cmd;
@@ -167,7 +165,17 @@ bool UDPSink::handleMessage(const Message& cmd)
     }
     else if (UDPSinkMessages::MsgSampleRateCorrection::match(cmd))
     {
-        // TODO
+        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);
+
+        m_settingsMutex.lock();
+        m_interpolatorDistanceRemain = 0;
+        m_interpolatorConsumed = false;
+        m_interpolatorDistance = (Real) m_actualInputSampleRate / (Real) m_config.m_outputSampleRate;
+        //m_interpolator.create(48, m_actualInputSampleRate, m_config.m_rfBandwidth / 2.2, 3.0); // causes clicking: leaving at standard frequency
+        m_settingsMutex.unlock();
+
         return true;
     }
     else
@@ -223,6 +231,8 @@ void UDPSink::apply(bool force)
         m_interpolatorConsumed = false;
         m_interpolatorDistance = (Real) m_config.m_inputSampleRate / (Real) m_config.m_outputSampleRate;
         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_settingsMutex.unlock();
     }
 
diff --git a/plugins/channeltx/udpsink/udpsink.h b/plugins/channeltx/udpsink/udpsink.h
index d3e4adcc2..8ea12c53a 100644
--- a/plugins/channeltx/udpsink/udpsink.h
+++ b/plugins/channeltx/udpsink/udpsink.h
@@ -198,6 +198,7 @@ private:
     MovingAverage<double> m_movingAverage;
 
     UDPSinkUDPHandler m_udpHandler;
+    Real m_actualInputSampleRate; //!< sample rate with UDP buffer skew compensation
 
     QMutex m_settingsMutex;
 
diff --git a/plugins/channeltx/udpsink/udpsinkudphandler.cpp b/plugins/channeltx/udpsink/udpsinkudphandler.cpp
index 1db891f4e..a9fe7b795 100644
--- a/plugins/channeltx/udpsink/udpsinkudphandler.cpp
+++ b/plugins/channeltx/udpsink/udpsinkudphandler.cpp
@@ -16,7 +16,9 @@
 
 #include <QDebug>
 
+#include "udpsinkmsg.h"
 #include "udpsinkudphandler.h"
+#include "util/messagequeue.h"
 
 UDPSinkUDPHandler::UDPSinkUDPHandler() :
     m_dataSocket(0),
@@ -29,6 +31,7 @@ UDPSinkUDPHandler::UDPSinkUDPHandler() :
     m_readFrameIndex(m_nbUDPFrames/2),
     m_readIndex(0),
     m_rwDelta(m_nbUDPFrames/2),
+    m_d(0),
     m_feedbackMessageQueue(0)
 {
 }
@@ -136,8 +139,26 @@ void UDPSinkUDPHandler::advanceReadPointer(int nbBytes)
         {
             m_rwDelta = m_writeIndex; // raw R/W delta estimate
             float d = (m_rwDelta - (m_nbUDPFrames/2))/(float) m_nbUDPFrames;
-            qDebug("UDPSinkUDPHandler::advanceReadPointer: w: %02d d: %f", m_writeIndex, d);
-            m_readFrameIndex = 0;
+            //qDebug("UDPSinkUDPHandler::advanceReadPointer: w: %02d d: %f", m_writeIndex, d);
+
+            if ((d < -0.45) || (d > 0.45))
+            {
+                resetReadIndex();
+            }
+            else
+            {
+                float dd = d - m_d; // derivative
+                float c = (d + dd) / 10.0; // (d / 10.0) + (dd / 10.0); // damping and scaling
+                c = c < -0.05 ? -0.05 : c > 0.05 ? 0.05 : c; // limit
+                UDPSinkMessages::MsgSampleRateCorrection *msg = UDPSinkMessages::MsgSampleRateCorrection::create(c);
+
+                if (m_feedbackMessageQueue) {
+                    m_feedbackMessageQueue->push(msg);
+                }
+
+                m_readFrameIndex = 0;
+                m_d = d;
+            }
         }
     }
 }
@@ -164,4 +185,5 @@ void UDPSinkUDPHandler::resetReadIndex()
     m_readFrameIndex = (m_writeIndex + (m_nbUDPFrames/2)) % m_nbUDPFrames;
     m_rwDelta = m_nbUDPFrames/2;
     m_readIndex = 0;
+    m_d = 0.0f;
 }
diff --git a/plugins/channeltx/udpsink/udpsinkudphandler.h b/plugins/channeltx/udpsink/udpsinkudphandler.h
index a426f45a0..a95be48bf 100644
--- a/plugins/channeltx/udpsink/udpsinkudphandler.h
+++ b/plugins/channeltx/udpsink/udpsinkudphandler.h
@@ -54,7 +54,7 @@ public:
     }
 
     static const int m_udpBlockSize = 512; // UDP block size in number of bytes
-    static const int m_nbUDPFrames = 128;  // number of frames of block size in the UDP buffer
+    static const int m_nbUDPFrames = 256;  // number of frames of block size in the UDP buffer
 
 public slots:
     void dataReadyRead();
@@ -75,6 +75,7 @@ private:
     int m_readFrameIndex;
     int m_readIndex;
     int m_rwDelta;
+    float m_d;
     MessageQueue *m_feedbackMessageQueue;
 };