diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp index 3b2d80735..504467dac 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp +++ b/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 or middle of raw buffer + if (m_readChunkIndex == m_rateDivider * 2) // go back to start of raw buffer { - // make sure the read and write pointers are not in the same half of the raw buffer - if (m_writeIndex < m_rateDivider * m_chunkSize - 1) + double oneCycleSkew = 0; + + if (m_readCycles > 0) { - m_readChunkIndex = m_rateDivider; // go to middle + oneCycleSkew = (double) ((int) m_writeIndex - (int) m_lastWriteIndex) / (double) m_rawSize; + m_skewRateSum += oneCycleSkew; } - 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_readChunkIndex = 0; // go to start + 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 diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h index 8e48aaf9d..e3c9a4bae 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h +++ b/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,9 +102,10 @@ private: uint32_t m_nbLz4CRCOK; uint64_t m_dataCRC; - uint32_t m_sampleRate; //!< Current 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 + 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 uint32_t m_writeIndex; //!< Current write position in the raw samples buffer uint32_t m_readChunkIndex; //!< Current read chunk index in the raw samples buffer @@ -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; }; diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp index 15f869591..231006c54 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp +++ b/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); - m_samplerate = metaData.m_sampleRate; + setSamplerate(sampleRate); + m_samplerate = sampleRate; change = true; }