SDRdaemon plugin: auto skew correction #1

plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp

@@ -39,7 +39,8 @@ SDRdaemonBuffer::SDRdaemonBuffer(uint32_t rateDivider) :
 	m_nbLz4SuccessfulDecodes(0),
 	m_nbLz4CRCOK(0),
 	m_dataCRC(0),
-	m_sampleRate(1000000),
+	m_sampleRateStream(0),
+	m_sampleRate(0),
 	m_sampleBytes(2),
 	m_sampleBits(12),
 	m_writeIndex(0),
@@ -48,7 +49,11 @@ SDRdaemonBuffer::SDRdaemonBuffer(uint32_t rateDivider) :
 	m_rawBuffer(0),
 	m_chunkSize(0),
 	m_bytesInBlock(0),
-	m_nbBlocks(0)
+	m_nbBlocks(0),
+	m_readCycles(0),
+	m_lastWriteIndex(0),
+	m_skewRateSum(0.0),
+	m_skewRate(0.0)
 {
 	m_currentMeta.init();
 }
@@ -106,6 +111,13 @@ bool SDRdaemonBuffer::readMeta(char *array, uint32_t length)
 			uint32_t frameSize = m_iqSampleSize * metaData->m_nbSamples * metaData->m_nbBlocks;
 			uint32_t sampleRate = metaData->m_sampleRate;
 
+			if (sampleRate != m_sampleRateStream)
+			{
+				m_sampleRateStream = sampleRate;
+			}
+
+			sampleRate += (((int) (sampleRate * m_skewRate)) / m_rateDivider) * m_rateDivider;
+
 			if (metaData->m_sampleBytes & 0x10)
 			{
 				m_lz4 = true;
@@ -240,18 +252,32 @@ void SDRdaemonBuffer::writeToRawBufferLZ4(const char *array, uint32_t length)
 uint8_t *SDRdaemonBuffer::readDataChunk()
 {
 	// relies on the fact that we always have an integer number of chunks in the raw buffer
+	if (m_readChunkIndex == m_rateDivider * 2) // go back to start of raw buffer
+	{
+		double oneCycleSkew = 0;
 
-	if (m_readChunkIndex == m_rateDivider * 2) // go back to start or middle of raw buffer
+		if (m_readCycles > 0)
 		{
-		// make sure the read and write pointers are not in the same half of the raw buffer
+			oneCycleSkew = (double) ((int) m_writeIndex - (int) m_lastWriteIndex) / (double) m_rawSize;
-		if (m_writeIndex < m_rateDivider * m_chunkSize - 1)
+			m_skewRateSum += oneCycleSkew;
-		{
-			m_readChunkIndex = m_rateDivider; // go to middle
 		}
-		else
+
+		//qDebug("SDRdaemonBuffer::readDataChunk: %d / %d (%lf)", m_writeIndex, m_rawSize, oneCycleSkew);
+
+		if (m_readCycles && ((m_writeIndex < m_rawSize / 10) || (m_rawSize - m_writeIndex < m_rawSize / 10)))
 		{
+			m_skewRate = m_skewRateSum / m_readCycles;
+			if (m_skewRate > 0.04) {
+				m_skewRate = 0.04;
+			} else if (m_skewRate < -0.04) {
+				m_skewRate = -0.04;
+			}
+			qDebug("SDRdaemonBuffer::readDataChunk: m_skewRate: %lf", m_skewRate);
+		}
+
 		m_readChunkIndex = 0; // go to start
-		}
+		m_lastWriteIndex = m_writeIndex;
+		m_readCycles++;
 	}
 
 	uint32_t readIndex = m_readChunkIndex;
@@ -292,6 +318,8 @@ void SDRdaemonBuffer::updateBufferSize(uint32_t sampleRate)
 
 	m_writeIndex = 0;
 	m_readChunkIndex = m_rateDivider;
+	m_readCycles = 0;
+	m_skewRateSum = 0;
 
 	std::cerr << "SDRdaemonBuffer::updateBufferSize:"
 		<< " sampleRate: " << sampleRate

plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h

@@ -67,6 +67,7 @@ public:
 	void writeData(char *array, uint32_t length); //!< Write data into buffer.
 	uint8_t *readDataChunk();                     //!< Read a chunk of data from buffer
 	const MetaData& getCurrentMeta() const { return m_currentMeta; }
+	uint32_t getSampleRate() const { return m_sampleRate; }
 	void updateBlockCounts(uint32_t nbBytesReceived);
 	bool isSync() const { return m_sync; }
 	bool isSyncLocked() const { return m_syncLock; }
@@ -101,7 +102,8 @@ private:
     uint32_t m_nbLz4CRCOK;
     uint64_t m_dataCRC;
 
-    uint32_t m_sampleRate;   //!< Current sample rate in Hz
+    uint32_t m_sampleRateStream; //!< Current sample rate from the stream
+    uint32_t m_sampleRate;       //!< Current actual sample rate in Hz
 	uint8_t  m_sampleBytes;      //!< Current number of bytes per I or Q sample
 	uint8_t  m_sampleBits;       //!< Current number of effective bits per sample
 
@@ -112,6 +114,11 @@ private:
     uint32_t m_chunkSize;    //!< Size of a chunk of samples in bytes
     uint32_t m_bytesInBlock; //!< Number of bytes received in the current UDP block
     uint32_t m_nbBlocks;     //!< Number of UDP blocks received in the current frame
+
+    uint32_t m_readCycles;     //!< Count of read cycles over raw buiffer
+    uint32_t m_lastWriteIndex; //!< Write index at last skew estimation
+    double   m_skewRateSum;
+    double   m_skewRate;
 };
 
 

plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp

@@ -137,10 +137,12 @@ void SDRdaemonUDPHandler::processData()
 			m_tv_sec = metaData.m_tv_sec;
 			m_tv_usec = metaData.m_tv_usec;
 
-			if (m_samplerate != metaData.m_sampleRate)
+			uint32_t sampleRate = m_sdrDaemonBuffer.getSampleRate();
+
+			if (m_samplerate != sampleRate)
 			{
-				setSamplerate(metaData.m_sampleRate);
+				setSamplerate(sampleRate);
-				m_samplerate = metaData.m_sampleRate;
+				m_samplerate = sampleRate;
 				change = true;
 			}