diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp index e1a85a73b..d84b96246 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp +++ b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.cpp @@ -14,25 +14,37 @@ // along with this program. If not, see . // /////////////////////////////////////////////////////////////////////////////////// +#include "sdrdaemonbuffer.h" + +#include #include #include -#include -#include -#include "sdrdaemonbuffer.h" +#include + const int SDRdaemonBuffer::m_udpPayloadSize = 512; const int SDRdaemonBuffer::m_sampleSize = 2; const int SDRdaemonBuffer::m_iqSampleSize = 2 * m_sampleSize; +const int SDRdaemonBuffer::m_rawBufferLengthSeconds = 4; // should be even -SDRdaemonBuffer::SDRdaemonBuffer(uint32_t rateDivider) : - m_rateDivider(rateDivider), +SDRdaemonBuffer::SDRdaemonBuffer(uint32_t throttlems) : + m_throttlemsNominal(throttlems), + m_rawSize(0), + m_rawBuffer(0), + m_sampleRateStream(0), + m_sampleRate(0), + m_sampleBytes(2), + m_sampleBits(12), m_sync(false), m_syncLock(false), m_lz4(false), + m_nbBlocks(0), + m_bytesInBlock(0), + m_dataCRC(0), m_inCount(0), - m_lz4InBuffer(0), m_lz4InCount(0), m_lz4InSize(0), + m_lz4InBuffer(0), m_lz4OutBuffer(0), m_frameSize(0), m_nbLz4Decodes(0), @@ -40,22 +52,10 @@ SDRdaemonBuffer::SDRdaemonBuffer(uint32_t rateDivider) : m_nbLz4CRCOK(0), m_nbLastLz4SuccessfulDecodes(0), m_nbLastLz4CRCOK(0), - m_dataCRC(0), - m_sampleRateStream(0), - m_sampleRate(0), - m_sampleBytes(2), - m_sampleBits(12), m_writeIndex(0), - m_readChunkIndex(0), - m_rawSize(0), - m_rawBuffer(0), - m_chunkSize(0), - m_bytesInBlock(0), - m_nbBlocks(0), - m_readCycles(0), - m_lastWriteIndex(0), - m_skewRateSum(0.0), - m_skewRate(0.0), + m_readIndex(0), + m_readSize(0), + m_readBuffer(0), m_autoFollowRate(false) { m_currentMeta.init(); @@ -74,6 +74,58 @@ SDRdaemonBuffer::~SDRdaemonBuffer() if (m_lz4OutBuffer) { delete[] m_lz4OutBuffer; } + + if (m_readBuffer) { + delete[] m_readBuffer; + } +} + +void SDRdaemonBuffer::updateBufferSize(uint32_t sampleRate) +{ + uint32_t rawSize = sampleRate * m_iqSampleSize * m_rawBufferLengthSeconds; // store worth of this seconds of samples at this sample rate + + if (rawSize != m_rawSize) + { + m_rawSize = rawSize; + + if (m_rawBuffer) { + delete[] m_rawBuffer; + } + + m_rawBuffer = new uint8_t[m_rawSize]; + m_writeIndex = 0; + m_readIndex = m_rawSize / 2; + + qDebug() << "SDRdaemonBuffer::updateBufferSize:" + << " sampleRate: " << sampleRate + << " m_rawSize: " << m_rawSize; + } +} + +void SDRdaemonBuffer::updateLZ4Sizes(uint32_t frameSize) +{ + uint32_t maxInputSize = LZ4_compressBound(frameSize); + + if (m_lz4InBuffer) { + delete[] m_lz4InBuffer; + } + + m_lz4InBuffer = new uint8_t[maxInputSize]; + + if (m_lz4OutBuffer) { + delete[] m_lz4OutBuffer; + } + + m_lz4OutBuffer = new uint8_t[frameSize]; +} + +void SDRdaemonBuffer::updateReadBufferSize(uint32_t length) +{ + if (m_readBuffer) { + delete[] m_readBuffer; + } + + m_readBuffer = new uint8_t[length]; } bool SDRdaemonBuffer::readMeta(char *array, uint32_t length) @@ -81,7 +133,7 @@ bool SDRdaemonBuffer::readMeta(char *array, uint32_t length) assert(length >= sizeof(MetaData) + 8); MetaData *metaData = (MetaData *) array; - if (m_crc64.calculate_crc((uint8_t *)array, sizeof(MetaData) - 8) == metaData->m_crc) + if (m_crc64.calculate_crc((uint8_t *) array, sizeof(MetaData) - 8) == metaData->m_crc) { // sync condition: if (m_currentMeta.m_blockSize > 0) @@ -101,8 +153,7 @@ bool SDRdaemonBuffer::readMeta(char *array, uint32_t length) if (!m_lz4 && !(m_currentMeta == *metaData)) { - std::cerr << "SDRdaemonBuffer::readMeta: "; - printMeta(metaData); + printMeta(QString("SDRdaemonBuffer::readMeta"), metaData); } m_currentMeta = *metaData; @@ -114,23 +165,12 @@ 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 (m_autoFollowRate) - { - if (sampleRate != m_sampleRateStream) - { - m_sampleRateStream = sampleRate; - } - else - { - sampleRate = m_sampleRate; - } - - sampleRate += sampleRate * m_skewRate; - sampleRate = (sampleRate / m_rateDivider) * m_rateDivider; - } - else + if (sampleRate != m_sampleRateStream) { + updateBufferSize(sampleRate); m_sampleRateStream = sampleRate; + m_sampleRate = sampleRate; + // TODO: auto skew rate compensation } if (metaData->m_sampleBytes & 0x10) @@ -149,12 +189,6 @@ bool SDRdaemonBuffer::readMeta(char *array, uint32_t length) m_lz4 = false; } - if (sampleRate != m_sampleRate) - { - updateBufferSize(sampleRate); - } - - m_sampleRate = sampleRate; m_frameSize = frameSize; m_sync = true; } @@ -186,6 +220,38 @@ void SDRdaemonBuffer::writeData(char *array, uint32_t length) } } +uint8_t *SDRdaemonBuffer::readData(int32_t length) +{ + if (m_readIndex + length < m_rawSize) + { + uint32_t readIndex = m_readIndex; + m_readIndex += length; + return &m_rawBuffer[readIndex]; + } + else if (m_readIndex + length == m_rawSize) + { + uint32_t readIndex = m_readIndex; + m_readIndex = 0; + return &m_rawBuffer[readIndex]; + } + else + { + // TODO: implement auto skew rate compensation calculation + + if (length > m_readSize) + { + updateReadBufferSize(length); + m_readSize = length; + } + + std::memcpy((void *) m_readBuffer, (const void *) &m_rawBuffer[m_readIndex], m_rawSize - m_readIndex); + 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; + } +} + void SDRdaemonBuffer::writeDataLZ4(const char *array, uint32_t length) { if (m_lz4InCount + length < m_lz4InSize) @@ -203,11 +269,10 @@ void SDRdaemonBuffer::writeDataLZ4(const char *array, uint32_t length) { if (m_nbLz4Decodes == 100) { - std::cerr << "SDRdaemonBuffer::writeAndReadLZ4:" + qDebug() << "SDRdaemonBuffer::writeAndReadLZ4:" << " decoding: " << m_nbLz4CRCOK << ":" << m_nbLz4SuccessfulDecodes - << "/" << m_nbLz4Decodes - << std::endl; + << "/" << m_nbLz4Decodes; m_nbLastLz4SuccessfulDecodes = m_nbLz4SuccessfulDecodes; m_nbLastLz4CRCOK = m_nbLz4CRCOK; @@ -221,23 +286,6 @@ void SDRdaemonBuffer::writeDataLZ4(const char *array, uint32_t length) } } -void SDRdaemonBuffer::writeToRawBufferUncompressed(const char *array, uint32_t length) -{ - // TODO: handle the 1 byte per I or Q sample - if (m_writeIndex + length < m_rawSize) - { - std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, length); - m_writeIndex += length; - } - else - { - std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, m_rawSize - m_writeIndex); - length -= m_rawSize - m_writeIndex; - std::memcpy((void *) m_rawBuffer, (const void *) &array[m_rawSize - m_writeIndex], length); - m_writeIndex = length; - } -} - void SDRdaemonBuffer::writeToRawBufferLZ4() { uint64_t crc64 = m_crc64.calculate_crc(m_lz4InBuffer, m_lz4InSize); @@ -261,88 +309,26 @@ void SDRdaemonBuffer::writeToRawBufferLZ4() } } -uint8_t *SDRdaemonBuffer::readDataChunk() +void SDRdaemonBuffer::writeToRawBufferUncompressed(const char *array, uint32_t length) { - // 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 + // TODO: handle the 1 byte per I or Q sample + if (m_writeIndex + length < m_rawSize) { - double oneCycleSkew = 0; - - if (m_readCycles > 0) - { - oneCycleSkew = (double) ((int) m_writeIndex - (int) m_lastWriteIndex) / (double) m_rawSize; - m_skewRateSum += oneCycleSkew; - } - - //qDebug("SDRdaemonBuffer::readDataChunk: %d / %d (%lf)", m_writeIndex, m_rawSize, oneCycleSkew); - - if (!m_autoFollowRate) - { - m_skewRate = 0.0; - } - else 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.2) { - m_skewRate = 0.2; - } else if (m_skewRate < -0.2) { - m_skewRate = -0.2; - } - qDebug("SDRdaemonBuffer::readDataChunk: m_skewRate: %lf", m_skewRate); - } - - m_readChunkIndex = 0; // go to start - m_lastWriteIndex = m_writeIndex; - m_readCycles++; + std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, length); + m_writeIndex += length; } - - uint32_t readIndex = m_readChunkIndex; - m_readChunkIndex++; - return &m_rawBuffer[readIndex * m_chunkSize]; -} - -void SDRdaemonBuffer::updateLZ4Sizes(uint32_t frameSize) -{ - uint32_t maxInputSize = LZ4_compressBound(frameSize); - - if (m_lz4InBuffer) { - delete[] m_lz4InBuffer; + else if (m_writeIndex + length == m_rawSize) + { + std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, length); + m_writeIndex = 0; } - - m_lz4InBuffer = new uint8_t[maxInputSize]; - - if (m_lz4OutBuffer) { - delete[] m_lz4OutBuffer; + else + { + std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, m_rawSize - m_writeIndex); + length -= m_rawSize - m_writeIndex; + std::memcpy((void *) m_rawBuffer, (const void *) &array[m_rawSize - m_writeIndex], length); + m_writeIndex = length; } - - m_lz4OutBuffer = new uint8_t[frameSize]; -} - -void SDRdaemonBuffer::updateBufferSize(uint32_t sampleRate) -{ - assert(sampleRate % m_rateDivider == 0); // make sure we get an integer number of samples in a chunk - - // Store 2 seconds long of samples so we have two one second long half buffers - m_chunkSize = (sampleRate * m_iqSampleSize) / m_rateDivider; - m_rawSize = m_chunkSize * m_rateDivider * 2; - - if (m_rawBuffer) { - delete[] m_rawBuffer; - } - - m_rawBuffer = new uint8_t[m_rawSize]; - - m_writeIndex = 0; - m_readChunkIndex = m_rateDivider; - m_readCycles = 0; - m_skewRateSum = 0; - m_skewRate = 0; - - std::cerr << "SDRdaemonBuffer::updateBufferSize:" - << " sampleRate: " << sampleRate - << " m_chunkSize: " << m_chunkSize - << " m_rawSize: " << m_rawSize - << std::endl; } void SDRdaemonBuffer::updateBlockCounts(uint32_t nbBytesReceived) @@ -351,19 +337,18 @@ void SDRdaemonBuffer::updateBlockCounts(uint32_t nbBytesReceived) m_bytesInBlock = m_bytesInBlock + nbBytesReceived > m_udpPayloadSize ? nbBytesReceived : m_bytesInBlock + nbBytesReceived; } -void SDRdaemonBuffer::printMeta(MetaData *metaData) +void SDRdaemonBuffer::printMeta(const QString& header, MetaData *metaData) { - std::cerr - << "|" << metaData->m_centerFrequency - << ":" << metaData->m_sampleRate - << ":" << (int) (metaData->m_sampleBytes & 0xF) - << ":" << (int) metaData->m_sampleBits - << ":" << metaData->m_blockSize - << ":" << metaData->m_nbSamples - << "||" << metaData->m_nbBlocks - << ":" << metaData->m_nbBytes - << "|" << metaData->m_tv_sec - << ":" << metaData->m_tv_usec - << std::endl; + qDebug() << header << ": " + << "|" << metaData->m_centerFrequency + << ":" << metaData->m_sampleRate + << ":" << (int) (metaData->m_sampleBytes & 0xF) + << ":" << (int) metaData->m_sampleBits + << ":" << metaData->m_blockSize + << ":" << metaData->m_nbSamples + << "||" << metaData->m_nbBlocks + << ":" << metaData->m_nbBytes + << "|" << metaData->m_tv_sec + << ":" << metaData->m_tv_usec; } diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h index 09bdbd26d..0474d4fa9 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h +++ b/plugins/samplesource/sdrdaemon/sdrdaemonbuffer.h @@ -17,16 +17,15 @@ #ifndef PLUGINS_SAMPLESOURCE_SDRDAEMON_SDRDAEMONBUFFER_H_ #define PLUGINS_SAMPLESOURCE_SDRDAEMON_SDRDAEMONBUFFER_H_ -#include -#include -#include -#include +#include +#include + #include "util/CRC64.h" -#include "dsp/samplefifo.h" class SDRdaemonBuffer { public: +public: #pragma pack(push, 1) struct MetaData { @@ -61,15 +60,17 @@ public: }; #pragma pack(pop) - SDRdaemonBuffer(uint32_t rateDivider); + SDRdaemonBuffer(uint32_t throttlems); ~SDRdaemonBuffer(); + bool readMeta(char *array, uint32_t length); //!< Attempt to read meta. Returns true if meta block void writeData(char *array, uint32_t length); //!< Write data into buffer. - uint8_t *readDataChunk(); //!< Read a chunk of data from buffer + uint8_t *readData(int32_t length); + void updateBlockCounts(uint32_t nbBytesReceived); + const MetaData& getCurrentMeta() const { return m_currentMeta; } uint32_t getSampleRateStream() const { return m_sampleRateStream; } uint32_t getSampleRate() const { return m_sampleRate; } - void updateBlockCounts(uint32_t nbBytesReceived); bool isSync() const { return m_sync; } bool isSyncLocked() const { return m_syncLock; } uint32_t getFrameSize() const { return m_frameSize; } @@ -82,26 +83,38 @@ public: static const int m_udpPayloadSize; static const int m_sampleSize; static const int m_iqSampleSize; + static const int m_rawBufferLengthSeconds; private: + void updateBufferSize(uint32_t sampleRate); void updateLZ4Sizes(uint32_t frameSize); + void updateReadBufferSize(uint32_t length); void writeDataLZ4(const char *array, uint32_t length); void writeToRawBufferLZ4(); void writeToRawBufferUncompressed(const char *array, uint32_t length); - void updateBufferSize(uint32_t sampleRate); - void printMeta(MetaData *metaData); - uint32_t m_rateDivider; //!< Number of times per seconds the samples are fetched + static void printMeta(const QString& header, MetaData *metaData); + + uint32_t m_throttlemsNominal; //!< Initial throttle in ms + uint32_t m_rawSize; //!< Size of the raw samples buffer in bytes + uint8_t *m_rawBuffer; //!< Buffer for raw samples obtained from UDP (I/Q not in a formal I/Q structure) + uint32_t m_sampleRateStream; //!< Current sample rate from the stream meta data + 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 + bool m_sync; //!< Meta data acquired bool m_syncLock; //!< Meta data expected (Stream synchronized) bool m_lz4; //!< Stream is compressed with LZ4 MetaData m_currentMeta; //!< Stored current meta data CRC64 m_crc64; //!< CRC64 calculator - + uint32_t m_nbBlocks; //!< Number of UDP blocks received in the current frame + uint32_t m_bytesInBlock; //!< Number of bytes received in the current UDP block + uint64_t m_dataCRC; //!< CRC64 of the data block uint32_t m_inCount; //!< Current position of uncompressed input - uint8_t *m_lz4InBuffer; //!< Buffer for LZ4 compressed input uint32_t m_lz4InCount; //!< Current position in LZ4 input buffer uint32_t m_lz4InSize; //!< Size in bytes of the LZ4 input data + uint8_t *m_lz4InBuffer; //!< Buffer for LZ4 compressed input uint8_t *m_lz4OutBuffer; //!< Buffer for LZ4 uncompressed output uint32_t m_frameSize; //!< Size in bytes of one uncompressed frame uint32_t m_nbLz4Decodes; @@ -109,26 +122,14 @@ private: uint32_t m_nbLz4CRCOK; uint32_t m_nbLastLz4SuccessfulDecodes; uint32_t m_nbLastLz4CRCOK; - uint64_t m_dataCRC; - 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 + int32_t m_writeIndex; //!< Current write position in the raw samples buffer + int32_t m_readIndex; //!< Current read position in the raw samples buffer + uint32_t m_readSize; //!< Read buffer size + uint8_t *m_readBuffer; //!< Read buffer to hold samples when looping back to beginning of raw buffer - 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 - uint32_t m_rawSize; //!< Size of the raw samples buffer in bytes - uint8_t *m_rawBuffer; //!< Buffer for raw samples obtained from UDP (I/Q not in a formal I/Q structure) - 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 + bool m_autoFollowRate; //!< Auto follow stream sample rate else stick with meta data sample rate - 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; - bool m_autoFollowRate; //!< Aito follow stream sample rate else stick with meta data sample rate }; diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.cpp new file mode 100644 index 000000000..d95034f38 --- /dev/null +++ b/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.cpp @@ -0,0 +1,370 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2016 Edouard Griffiths, F4EXB // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#include "sdrdaemonbufferold.h" + +#include +#include +#include +#include + +const int SDRdaemonBufferOld::m_udpPayloadSize = 512; +const int SDRdaemonBufferOld::m_sampleSize = 2; +const int SDRdaemonBufferOld::m_iqSampleSize = 2 * m_sampleSize; + +SDRdaemonBufferOld::SDRdaemonBufferOld(uint32_t rateDivider) : + m_rateDivider(rateDivider), + m_sync(false), + m_syncLock(false), + m_lz4(false), + m_inCount(0), + m_lz4InBuffer(0), + m_lz4InCount(0), + m_lz4InSize(0), + m_lz4OutBuffer(0), + m_frameSize(0), + m_nbLz4Decodes(0), + m_nbLz4SuccessfulDecodes(0), + m_nbLz4CRCOK(0), + m_nbLastLz4SuccessfulDecodes(0), + m_nbLastLz4CRCOK(0), + m_dataCRC(0), + m_sampleRateStream(0), + m_sampleRate(0), + m_sampleBytes(2), + m_sampleBits(12), + m_writeIndex(0), + m_readChunkIndex(0), + m_rawSize(0), + m_rawBuffer(0), + m_chunkSize(0), + m_bytesInBlock(0), + m_nbBlocks(0), + m_readCycles(0), + m_lastWriteIndex(0), + m_skewRateSum(0.0), + m_skewRate(0.0), + m_autoFollowRate(false) +{ + m_currentMeta.init(); +} + +SDRdaemonBufferOld::~SDRdaemonBufferOld() +{ + if (m_rawBuffer) { + delete[] m_rawBuffer; + } + + if (m_lz4InBuffer) { + delete[] m_lz4InBuffer; + } + + if (m_lz4OutBuffer) { + delete[] m_lz4OutBuffer; + } +} + +bool SDRdaemonBufferOld::readMeta(char *array, uint32_t length) +{ + assert(length >= sizeof(MetaData) + 8); + MetaData *metaData = (MetaData *) array; + + if (m_crc64.calculate_crc((uint8_t *)array, sizeof(MetaData) - 8) == metaData->m_crc) + { + // sync condition: + if (m_currentMeta.m_blockSize > 0) + { + uint32_t nbBlocks = m_currentMeta.m_nbBytes / m_currentMeta.m_blockSize; + m_syncLock = nbBlocks + (m_lz4 ? 2 : 1) == m_nbBlocks; + //qDebug("SDRdaemonBuffer::readMeta: m_nbBlocks: %d:%d %s", nbBlocks, m_nbBlocks, (m_syncLock ? "locked" : "unlocked")); + } + else + { + m_syncLock = false; + } + + memcpy((void *) &m_dataCRC, (const void *) &array[sizeof(MetaData)], 8); + m_nbBlocks = 0; + m_inCount = 0; + + if (!m_lz4 && !(m_currentMeta == *metaData)) + { + std::cerr << "SDRdaemonBuffer::readMeta: "; + printMeta(metaData); + } + + m_currentMeta = *metaData; + + // sanity checks + if (metaData->m_blockSize == m_udpPayloadSize) // sent blocksize matches given blocksize + { + m_sampleBytes = metaData->m_sampleBytes & 0x0F; + uint32_t frameSize = m_iqSampleSize * metaData->m_nbSamples * metaData->m_nbBlocks; + int sampleRate = metaData->m_sampleRate; + + if (m_autoFollowRate) + { + if (sampleRate != m_sampleRateStream) + { + m_sampleRateStream = sampleRate; + } + else + { + sampleRate = m_sampleRate; + } + + sampleRate += sampleRate * m_skewRate; + sampleRate = (sampleRate / m_rateDivider) * m_rateDivider; + } + else + { + m_sampleRateStream = sampleRate; + } + + if (metaData->m_sampleBytes & 0x10) + { + m_lz4 = true; + m_lz4InSize = metaData->m_nbBytes; // compressed input size + m_lz4InCount = 0; + + if (frameSize != m_frameSize) + { + updateLZ4Sizes(frameSize); + } + } + else + { + m_lz4 = false; + } + + if (sampleRate != m_sampleRate) + { + updateBufferSize(sampleRate); + } + + m_sampleRate = sampleRate; + m_frameSize = frameSize; + m_sync = true; + } + else + { + m_sync = false; + } + + return m_sync; + } + else + { + return false; + } +} + +void SDRdaemonBufferOld::writeData(char *array, uint32_t length) +{ + if ((m_sync) && (m_nbBlocks > 0)) + { + if (m_lz4) + { + writeDataLZ4(array, length); + } + else + { + writeToRawBufferUncompressed(array, length); + } + } +} + +void SDRdaemonBufferOld::writeDataLZ4(const char *array, uint32_t length) +{ + if (m_lz4InCount + length < m_lz4InSize) + { + std::memcpy((void *) &m_lz4InBuffer[m_lz4InCount], (const void *) array, length); + m_lz4InCount += length; + } + else + { + std::memcpy((void *) &m_lz4InBuffer[m_lz4InCount], (const void *) array, m_lz4InSize - m_lz4InCount); // copy rest of data in compressed Buffer + m_lz4InCount += length; + } + + if (m_lz4InCount >= m_lz4InSize) // full input compressed block retrieved + { + if (m_nbLz4Decodes == 100) + { + std::cerr << "SDRdaemonBuffer::writeAndReadLZ4:" + << " decoding: " << m_nbLz4CRCOK + << ":" << m_nbLz4SuccessfulDecodes + << "/" << m_nbLz4Decodes + << std::endl; + + m_nbLastLz4SuccessfulDecodes = m_nbLz4SuccessfulDecodes; + m_nbLastLz4CRCOK = m_nbLz4CRCOK; + m_nbLz4Decodes = 0; + m_nbLz4SuccessfulDecodes = 0; + m_nbLz4CRCOK = 0; + } + + writeToRawBufferLZ4(); + m_lz4InCount = 0; + } +} + +void SDRdaemonBufferOld::writeToRawBufferUncompressed(const char *array, uint32_t length) +{ + // TODO: handle the 1 byte per I or Q sample + if (m_writeIndex + length < m_rawSize) + { + std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, length); + m_writeIndex += length; + } + else + { + std::memcpy((void *) &m_rawBuffer[m_writeIndex], (const void *) array, m_rawSize - m_writeIndex); + length -= m_rawSize - m_writeIndex; + std::memcpy((void *) m_rawBuffer, (const void *) &array[m_rawSize - m_writeIndex], length); + m_writeIndex = length; + } +} + +void SDRdaemonBufferOld::writeToRawBufferLZ4() +{ + uint64_t crc64 = m_crc64.calculate_crc(m_lz4InBuffer, m_lz4InSize); + + if (memcmp(&crc64, &m_dataCRC, 8) == 0) + { + m_nbLz4CRCOK++; + } + else + { + return; + } + + int compressedSize = LZ4_decompress_fast((const char*) m_lz4InBuffer, (char*) m_lz4OutBuffer, m_frameSize); + m_nbLz4Decodes++; + + if (compressedSize == m_lz4InSize) + { + m_nbLz4SuccessfulDecodes++; + writeToRawBufferUncompressed((const char *) m_lz4OutBuffer, m_frameSize); + } +} + +uint8_t *SDRdaemonBufferOld::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_readCycles > 0) + { + oneCycleSkew = (double) ((int) m_writeIndex - (int) m_lastWriteIndex) / (double) m_rawSize; + m_skewRateSum += oneCycleSkew; + } + + //qDebug("SDRdaemonBuffer::readDataChunk: %d / %d (%lf)", m_writeIndex, m_rawSize, oneCycleSkew); + + if (!m_autoFollowRate) + { + m_skewRate = 0.0; + } + else 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.2) { + m_skewRate = 0.2; + } else if (m_skewRate < -0.2) { + m_skewRate = -0.2; + } + 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; + m_readChunkIndex++; + return &m_rawBuffer[readIndex * m_chunkSize]; +} + +void SDRdaemonBufferOld::updateLZ4Sizes(uint32_t frameSize) +{ + uint32_t maxInputSize = LZ4_compressBound(frameSize); + + if (m_lz4InBuffer) { + delete[] m_lz4InBuffer; + } + + m_lz4InBuffer = new uint8_t[maxInputSize]; + + if (m_lz4OutBuffer) { + delete[] m_lz4OutBuffer; + } + + m_lz4OutBuffer = new uint8_t[frameSize]; +} + +void SDRdaemonBufferOld::updateBufferSize(uint32_t sampleRate) +{ + assert(sampleRate % m_rateDivider == 0); // make sure we get an integer number of samples in a chunk + + // Store 2 seconds long of samples so we have two one second long half buffers + m_chunkSize = (sampleRate * m_iqSampleSize) / m_rateDivider; + m_rawSize = m_chunkSize * m_rateDivider * 2; + + if (m_rawBuffer) { + delete[] m_rawBuffer; + } + + m_rawBuffer = new uint8_t[m_rawSize]; + + m_writeIndex = 0; + m_readChunkIndex = m_rateDivider; + m_readCycles = 0; + m_skewRateSum = 0; + m_skewRate = 0; + + std::cerr << "SDRdaemonBuffer::updateBufferSize:" + << " sampleRate: " << sampleRate + << " m_chunkSize: " << m_chunkSize + << " m_rawSize: " << m_rawSize + << std::endl; +} + +void SDRdaemonBufferOld::updateBlockCounts(uint32_t nbBytesReceived) +{ + m_nbBlocks += m_bytesInBlock + nbBytesReceived > m_udpPayloadSize ? 1 : 0; + m_bytesInBlock = m_bytesInBlock + nbBytesReceived > m_udpPayloadSize ? nbBytesReceived : m_bytesInBlock + nbBytesReceived; +} + +void SDRdaemonBufferOld::printMeta(MetaData *metaData) +{ + std::cerr + << "|" << metaData->m_centerFrequency + << ":" << metaData->m_sampleRate + << ":" << (int) (metaData->m_sampleBytes & 0xF) + << ":" << (int) metaData->m_sampleBits + << ":" << metaData->m_blockSize + << ":" << metaData->m_nbSamples + << "||" << metaData->m_nbBlocks + << ":" << metaData->m_nbBytes + << "|" << metaData->m_tv_sec + << ":" << metaData->m_tv_usec + << std::endl; +} + diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.h b/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.h new file mode 100644 index 000000000..57aeebddc --- /dev/null +++ b/plugins/samplesource/sdrdaemon/sdrdaemonbufferold.h @@ -0,0 +1,136 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2016 Edouard Griffiths, F4EXB // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef PLUGINS_SAMPLESOURCE_SDRDAEMON_SDRDAEMONBUFFEROLD_H_ +#define PLUGINS_SAMPLESOURCE_SDRDAEMON_SDRDAEMONBUFFEROLD_H_ + +#include +#include +#include +#include +#include "util/CRC64.h" +#include "dsp/samplefifo.h" + +class SDRdaemonBufferOld +{ +public: +#pragma pack(push, 1) + struct MetaData + { + // critical data + uint32_t m_centerFrequency; //!< center frequency in kHz + uint32_t m_sampleRate; //!< sample rate in Hz + uint8_t m_sampleBytes; //!< MSB(4): indicators, LSB(4) number of bytes per sample + uint8_t m_sampleBits; //!< number of effective bits per sample + uint16_t m_blockSize; //!< payload size + uint32_t m_nbSamples; //!< number of samples in a hardware block + // end of critical data + uint16_t m_nbBlocks; //!< number of hardware blocks in the frame + uint32_t m_nbBytes; //!< total number of bytes in the frame + uint32_t m_tv_sec; //!< seconds of timestamp at start time of frame processing + uint32_t m_tv_usec; //!< microseconds of timestamp at start time of frame processing + uint64_t m_crc; //!< 64 bit CRC of the above + + bool operator==(const MetaData& rhs) + { + return (memcmp((const void *) this, (const void *) &rhs, 20) == 0); // Only the 20 first bytes are relevant (critical) + } + + void init() + { + memset((void *) this, 0, sizeof(MetaData)); + } + + void operator=(const MetaData& rhs) + { + memcpy((void *) this, (const void *) &rhs, sizeof(MetaData)); + } + }; +#pragma pack(pop) + + SDRdaemonBufferOld(uint32_t rateDivider); + ~SDRdaemonBufferOld(); + bool readMeta(char *array, uint32_t length); //!< Attempt to read meta. Returns true if meta block + 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 getSampleRateStream() const { return m_sampleRateStream; } + uint32_t getSampleRate() const { return m_sampleRate; } + void updateBlockCounts(uint32_t nbBytesReceived); + bool isSync() const { return m_sync; } + bool isSyncLocked() const { return m_syncLock; } + uint32_t getFrameSize() const { return m_frameSize; } + bool isLz4Compressed() const { return m_lz4; } + float getCompressionRatio() const { return (m_frameSize > 0 ? (float) m_lz4InSize / (float) m_frameSize : 1.0); } + uint32_t getLz4DataCRCOK() const { return m_nbLastLz4CRCOK; } + uint32_t getLz4SuccessfulDecodes() const { return m_nbLastLz4SuccessfulDecodes; } + void setAutoFollowRate(bool autoFollowRate) { m_autoFollowRate = autoFollowRate; } + + static const int m_udpPayloadSize; + static const int m_sampleSize; + static const int m_iqSampleSize; + +private: + void updateLZ4Sizes(uint32_t frameSize); + void writeDataLZ4(const char *array, uint32_t length); + void writeToRawBufferLZ4(); + void writeToRawBufferUncompressed(const char *array, uint32_t length); + void updateBufferSize(uint32_t sampleRate); + void printMeta(MetaData *metaData); + + uint32_t m_rateDivider; //!< Number of times per seconds the samples are fetched + bool m_sync; //!< Meta data acquired + bool m_syncLock; //!< Meta data expected (Stream synchronized) + bool m_lz4; //!< Stream is compressed with LZ4 + MetaData m_currentMeta; //!< Stored current meta data + CRC64 m_crc64; //!< CRC64 calculator + + uint32_t m_inCount; //!< Current position of uncompressed input + uint8_t *m_lz4InBuffer; //!< Buffer for LZ4 compressed input + uint32_t m_lz4InCount; //!< Current position in LZ4 input buffer + uint32_t m_lz4InSize; //!< Size in bytes of the LZ4 input data + uint8_t *m_lz4OutBuffer; //!< Buffer for LZ4 uncompressed output + uint32_t m_frameSize; //!< Size in bytes of one uncompressed frame + uint32_t m_nbLz4Decodes; + uint32_t m_nbLz4SuccessfulDecodes; + uint32_t m_nbLz4CRCOK; + uint32_t m_nbLastLz4SuccessfulDecodes; + uint32_t m_nbLastLz4CRCOK; + uint64_t m_dataCRC; + + 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 + uint32_t m_rawSize; //!< Size of the raw samples buffer in bytes + uint8_t *m_rawBuffer; //!< Buffer for raw samples obtained from UDP (I/Q not in a formal I/Q structure) + 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; + bool m_autoFollowRate; //!< Aito follow stream sample rate else stick with meta data sample rate +}; + + + +#endif /* PLUGINS_SAMPLESOURCE_SDRDAEMON_SDRDAEMONBUFFEROLD_H_ */ diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp index d0af8a0ae..f5a13589a 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp +++ b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.cpp @@ -42,7 +42,8 @@ SDRdaemonUDPHandler::SDRdaemonUDPHandler(SampleFifo *sampleFifo, MessageQueue *o m_outputMessageQueueToGUI(outputMessageQueueToGUI), m_tickCount(0), m_samplesCount(0), - m_timer(0) + m_timer(0), + m_throttlems(SDRDAEMON_THROTTLE_MS) { m_udpBuf = new char[SDRdaemonBuffer::m_udpPayloadSize]; } @@ -199,9 +200,13 @@ void SDRdaemonUDPHandler::connectTimer(const QTimer* timer) void SDRdaemonUDPHandler::tick() { + // TOOD: auto throttling + uint32_t readLengthSamples = (m_sdrDaemonBuffer.getSampleRate() * m_throttlems) / 1000; + uint32_t readLength = readLengthSamples * SDRdaemonBuffer::m_iqSampleSize; + // read samples directly feeding the SampleFifo (no callback) - m_sampleFifo->write(reinterpret_cast(m_sdrDaemonBuffer.readDataChunk()), m_chunksize); - m_samplesCount += m_chunksize / SDRdaemonBuffer::m_iqSampleSize; + m_sampleFifo->write(reinterpret_cast(m_sdrDaemonBuffer.readData(readLength)), readLength); + m_samplesCount += readLengthSamples; if (m_tickCount < m_rateDivider) { diff --git a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h index 9e9647b26..281b6017e 100644 --- a/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h +++ b/plugins/samplesource/sdrdaemon/sdrdaemonudphandler.h @@ -21,6 +21,8 @@ #include #include #include +#include + #include "sdrdaemonbuffer.h" #define SDRDAEMON_THROTTLE_MS 50 @@ -61,6 +63,8 @@ private: uint32_t m_tickCount; std::size_t m_samplesCount; const QTimer *m_timer; + QElapsedTimer m_elapsedTimer; + int m_throttlems; static const int m_rateDivider;