From 026016c0d5daadc001f1a59a532440026cea0ceb Mon Sep 17 00:00:00 2001
From: Edouard Griffiths <egriffiths@amadeus.com>
Date: Tue, 15 Mar 2016 18:56:15 +0100
Subject: [PATCH] SDRdaemon: new auto skew rate compensation #1

---
 .../sdrdaemon/sdrdaemonbuffer.cpp             | 45 ++++++++++++++++---
 .../samplesource/sdrdaemon/sdrdaemonbuffer.h  |  6 ++-
 .../sdrdaemon/sdrdaemonudphandler.h           |  1 +
 3 files changed, 45 insertions(+), 7 deletions(-)

diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp
index d84b96246..62b6d2369 100644
--- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp
+++ b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp
@@ -56,7 +56,11 @@ SDRdaemonBuffer::SDRdaemonBuffer(uint32_t throttlems) :
 	m_readIndex(0),
 	m_readSize(0),
 	m_readBuffer(0),
-	m_autoFollowRate(false)
+    m_autoFollowRate(false),
+    m_skewTest(false),
+    m_skewCorrection(false),
+    m_readCount(0),
+    m_writeCount(0)
 {
 	m_currentMeta.init();
 }
@@ -93,8 +97,7 @@ void SDRdaemonBuffer::updateBufferSize(uint32_t sampleRate)
 		}
 
 		m_rawBuffer = new uint8_t[m_rawSize];
-		m_writeIndex = 0;
-		m_readIndex = m_rawSize / 2;
+        resetIndexes();
 
 		qDebug() << "SDRdaemonBuffer::updateBufferSize:"
 			<< " sampleRate: " << sampleRate
@@ -165,14 +168,21 @@ bool SDRdaemonBuffer::readMeta(char *array, uint32_t length)
 			uint32_t frameSize = m_iqSampleSize * metaData->m_nbSamples * metaData->m_nbBlocks;
 			int sampleRate = metaData->m_sampleRate;
 
-			if (sampleRate != m_sampleRateStream)
+            if (sampleRate != m_sampleRateStream) // change of nominal stream sample rate
 			{
 				updateBufferSize(sampleRate);
 				m_sampleRateStream = sampleRate;
 				m_sampleRate = sampleRate;
-				// TODO: auto skew rate compensation
 			}
 
+            // auto skew rate compensation
+            if (m_skewCorrection)
+            {
+                uint64_t newRate = (m_sampleRate * m_writeCount) / (m_readCount * m_iqSampleSize);
+                m_sampleRate = newRate * m_iqSampleSize; // ensure it is a multiple of the I/Q sample size
+                resetIndexes();
+            }
+
 			if (metaData->m_sampleBytes & 0x10)
 			{
 				m_lz4 = true;
@@ -222,6 +232,15 @@ void SDRdaemonBuffer::writeData(char *array, uint32_t length)
 
 uint8_t *SDRdaemonBuffer::readData(int32_t length)
 {
+    if (m_skewTest && ((m_readIndex + length) > (m_rawSize / 2)))
+    {
+        int dIndex = (m_readIndex - m_writeIndex > 0 ? m_readIndex - m_writeIndex : m_writeIndex - m_readIndex); // absolute delta
+        m_skewCorrection = (dIndex < m_rawSize / 10); // close by 10%
+        m_skewTest = false;
+    }
+
+    m_readCount += length;
+
 	if (m_readIndex + length < m_rawSize)
 	{
 		uint32_t readIndex = m_readIndex;
@@ -232,6 +251,7 @@ uint8_t *SDRdaemonBuffer::readData(int32_t length)
 	{
 		uint32_t readIndex = m_readIndex;
 		m_readIndex = 0;
+        m_skewTest = true;
 		return &m_rawBuffer[readIndex];
 	}
 	else
@@ -248,7 +268,8 @@ uint8_t *SDRdaemonBuffer::readData(int32_t length)
 		length -= m_rawSize - m_readIndex;
 		std::memcpy((void *) &m_readBuffer[m_rawSize - m_readIndex], (const void *) m_rawBuffer, length);
 		m_readIndex = length;
-		return m_readBuffer;
+        m_skewTest = true;
+        return m_readBuffer;
 	}
 }
 
@@ -329,6 +350,18 @@ void SDRdaemonBuffer::writeToRawBufferUncompressed(const char *array, uint32_t l
 		std::memcpy((void *) m_rawBuffer, (const void *) &array[m_rawSize - m_writeIndex], length);
 		m_writeIndex = length;
 	}
+
+    m_writeCount += length;
+}
+
+void SDRdaemonBuffer::resetIndexes()
+{
+    m_writeIndex = 0;
+    m_readIndex = m_rawSize / 2;
+    m_readCount = 0;
+    m_writeCount = 0;
+    m_skewTest = false;
+    m_skewCorrection = false;
 }
 
 void SDRdaemonBuffer::updateBlockCounts(uint32_t nbBytesReceived)
diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h
index 0474d4fa9..f71e4ae45 100644
--- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h
+++ b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h
@@ -92,6 +92,7 @@ private:
 	void writeDataLZ4(const char *array, uint32_t length);
 	void writeToRawBufferLZ4();
 	void writeToRawBufferUncompressed(const char *array, uint32_t length);
+    void resetIndexes();
 
     static void printMeta(const QString& header, MetaData *metaData);
 
@@ -129,7 +130,10 @@ private:
 	uint8_t  *m_readBuffer;  //!< Read buffer to hold samples when looping back to beginning of raw buffer
 
     bool     m_autoFollowRate; //!< Auto follow stream sample rate else stick with meta data sample rate
-
+    bool     m_skewTest;
+    bool     m_skewCorrection; //!< Do a skew rate correction at next meta data reception
+    uint64_t m_readCount;
+    uint64_t m_writeCount;
 };
 
 
diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h
index 7d58d1a44..5250e1bf2 100644
--- a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h
+++ b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h
@@ -63,6 +63,7 @@ private:
 	uint32_t m_tickCount;
 	std::size_t m_samplesCount;
 	const QTimer *m_timer;
+
 	QElapsedTimer m_elapsedTimer;
 	int m_throttlems;
     uint32_t m_readLengthSamples;