diff --git a/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.cpp b/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.cpp index cda24a6ec..5bbf7bd35 100644 --- a/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.cpp +++ b/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.cpp @@ -37,6 +37,7 @@ SDRdaemonFECBuffer::SDRdaemonFECBuffer(uint32_t throttlems) : m_bufferLenSec(0.0f) { m_currentMeta.init(); + m_outputMeta.init(); m_framesNbBytes = nbDecoderSlots * sizeof(BufferFrame); m_wrDeltaEstimate = m_framesNbBytes / 2; m_paramsCM256.BlockBytes = sizeof(ProtectedBlock); // never changes @@ -87,6 +88,22 @@ void SDRdaemonFECBuffer::checkSlotData(int slotIndex) << " m_blockCount: " << m_decoderSlots[slotIndex].m_blockCount << " m_recoveryCount: " << m_decoderSlots[slotIndex].m_recoveryCount; } + + if (m_decoderSlots[slotIndex].m_metaRetrieved) // meta data has been retrieved (direct or recovery) + { + if (!(m_decoderSlots[slotIndex].m_blockZero.m_metaData == m_currentMeta)) + { + int sampleRate = m_decoderSlots[slotIndex].m_blockZero.m_metaData.m_sampleRate; + + if (sampleRate > 0) { + m_bufferLenSec = (float) m_framesNbBytes / (float) sampleRate; + } + + printMeta("SDRdaemonFECBuffer::checkSlotData: new meta", &m_decoderSlots[slotIndex].m_blockZero.m_metaData); // print for change other than timestamp + } + + m_currentMeta = m_decoderSlots[slotIndex].m_blockZero.m_metaData; // renew current meta + } } void SDRdaemonFECBuffer::initDecodeSlot(int slotIndex) @@ -94,6 +111,12 @@ void SDRdaemonFECBuffer::initDecodeSlot(int slotIndex) int pseudoWriteIndex = slotIndex * sizeof(BufferFrame); m_wrDeltaEstimate = pseudoWriteIndex - m_readIndex; + if (m_decoderSlots[slotIndex].m_metaRetrieved) { // meta data was retrieved in the current slot + m_outputMeta = m_decoderSlots[slotIndex].m_blockZero.m_metaData; + } else { + m_outputMeta = m_currentMeta; // use stored current meta + } + // collect stats before voiding the slot m_curNbBlocks = m_decoderSlots[slotIndex].m_blockCount; m_curNbRecovery = m_decoderSlots[slotIndex].m_recoveryCount; @@ -105,13 +128,124 @@ void SDRdaemonFECBuffer::initDecodeSlot(int slotIndex) m_decoderSlots[slotIndex].m_decoded = false; m_decoderSlots[slotIndex].m_metaRetrieved = false; m_decoderSlots[slotIndex].m_blockZero.m_metaData.init(); - memset((void *) m_frames[slotIndex].m_blocks, 0, (m_nbOriginalBlocks - 1) * samplesPerBlock * sizeof(Sample)); + memset((void *) m_frames[slotIndex].m_blocks, 0, (m_nbOriginalBlocks - 1) * sizeof(ProtectedBlock)); + memset((void *) m_decoderSlots[slotIndex].m_recoveryBlocks, 1, m_nbOriginalBlocks * sizeof(ProtectedBlock)); } void SDRdaemonFECBuffer::writeData(char *array, uint32_t length) { assert(length == m_udpPayloadSize); + SuperBlock *superBlock = (SuperBlock *) array; + int frameIndex = superBlock->header.frameIndex; + int decoderIndex = frameIndex % nbDecoderSlots; + + if (m_frameHead == -1) // initial state + { + m_decoderIndexHead = decoderIndex; // new decoder slot head + m_frameHead = frameIndex; + initReadIndex(); // reset read index + initDecodeAllSlots(); // initialize all slots + } + else if (m_frameHead != frameIndex) // frame break => new frame starts + { + m_decoderIndexHead = decoderIndex; // new decoder slot head + m_frameHead = frameIndex; // new frame head + checkSlotData(decoderIndex); // check slot before re-init + initDecodeSlot(decoderIndex); // collect stats and re-initialize current slot + } + + int blockCount = m_decoderSlots[decoderIndex].m_blockCount; + int recoveryCount = m_decoderSlots[decoderIndex].m_recoveryCount; + int nbOriginalBlocks = 0; + + if (blockCount < m_nbOriginalBlocks) // not enough blocks to decode -> store data + { + int blockIndex = superBlock->header.blockIndex; + m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[blockCount].Index = blockIndex; + + if (blockIndex == 0) // first block with meta + { + //qDebug() << "SDRdaemonFECBuffer::writeData(0): frameIndex: " << frameIndex << " blockIndex: " << blockIndex; + ProtectedBlockZero *blockZero = (ProtectedBlockZero *) &superBlock->protectedBlock; + m_decoderSlots[decoderIndex].m_blockZero = *blockZero; + m_decoderSlots[decoderIndex].m_metaRetrieved = true; + m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[blockCount].Block = (void *) &m_decoderSlots[decoderIndex].m_blockZero; + } + else if (blockIndex < m_nbOriginalBlocks) // original data + { + //qDebug() << "SDRdaemonFECBuffer::writeData(<128): frameIndex: " << frameIndex << " blockIndex: " << blockIndex; + m_frames[decoderIndex].m_blocks[blockIndex - 1] = superBlock->protectedBlock; + m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[blockCount].Block = (void *) &m_frames[decoderIndex].m_blocks[blockIndex - 1]; + } + else // recovery data + { + qDebug() << "SDRdaemonFECBuffer::writeData(>=128): frameIndex: " << frameIndex << " blockIndex: " << blockIndex; + + if (recoveryCount == 0) + { + nbOriginalBlocks = blockCount; + } + + m_decoderSlots[decoderIndex].m_recoveryBlocks[recoveryCount] = superBlock->protectedBlock; + m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[blockCount].Block = (void *) &m_decoderSlots[decoderIndex].m_recoveryBlocks[recoveryCount]; + m_decoderSlots[decoderIndex].m_recoveryCount++; + } + } + else if (!m_decoderSlots[decoderIndex].m_decoded) // ready to decode and not decoded yet + { + if (m_decoderSlots[decoderIndex].m_recoveryCount > 0) // recovery data used => need to decode FEC + { + if (m_decoderSlots[decoderIndex].m_metaRetrieved) // block zero with its meta data has been received + { + m_paramsCM256.RecoveryCount = m_decoderSlots[decoderIndex].m_blockZero.m_metaData.m_nbFECBlocks; + } + else + { + m_paramsCM256.RecoveryCount = m_currentMeta.m_nbFECBlocks; // take last stored value for number of FEC blocks + } + + if (cm256_decode(m_paramsCM256, m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks)) // failure to decode + { + qDebug() << "SDRdaemonFECBuffer::writeData: CM256 decode error:" + << " BlockBytes: " << m_paramsCM256.BlockBytes + << " OriginalCount: " << m_paramsCM256.OriginalCount + << " RecoveryCount: " << m_paramsCM256.RecoveryCount + << " m_recoveryCount: " << m_decoderSlots[decoderIndex].m_recoveryCount; + } + else // success to decode + { + qDebug() << "SDRdaemonFECBuffer::writeData: CM256 decode success:" + << " nbOriginalBlocks: " << nbOriginalBlocks + << " m_recoveryCount: " << m_decoderSlots[decoderIndex].m_recoveryCount; + + for (int ir = 0; ir < m_decoderSlots[decoderIndex].m_recoveryCount; ir++) // recover lost blocks + { + int blockIndex = m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[nbOriginalBlocks+ir].Index; + + if (blockIndex > 0) // ignore meta data for now + { + m_frames[decoderIndex].m_blocks[blockIndex - 1] = m_decoderSlots[decoderIndex].m_recoveryBlocks[ir]; + } + + qDebug() << "SDRdaemonFECBuffer::writeData:" + << " recovered block #" << blockIndex + << " i[0]: " << m_decoderSlots[decoderIndex].m_recoveryBlocks[ir].samples[0].i + << " q[0]: " << m_decoderSlots[decoderIndex].m_recoveryBlocks[ir].samples[0].q; + } + } + } + + m_decoderSlots[decoderIndex].m_decoded = true; + } + + m_decoderSlots[decoderIndex].m_blockCount++; +} + +void SDRdaemonFECBuffer::writeData0(char *array, uint32_t length) +{ + assert(length == m_udpPayloadSize); + bool dataAvailable = false; SuperBlock *superBlock = (SuperBlock *) array; int frameIndex = superBlock->header.frameIndex; @@ -237,7 +371,6 @@ void SDRdaemonFECBuffer::writeData(char *array, uint32_t length) for (int ir = 0; ir < m_decoderSlots[decoderIndex].m_recoveryCount; ir++) // recover lost blocks { int blockIndex = m_decoderSlots[decoderIndex].m_cm256DescriptorBlocks[nbOriginalBlocks+ir].Index; - qDebug() << "SDRdaemonFECBuffer::writeData: recovered block #" << blockIndex; if (blockIndex == 0) { @@ -251,6 +384,11 @@ void SDRdaemonFECBuffer::writeData(char *array, uint32_t length) { m_frames[decoderIndex].m_blocks[blockIndex - 1] = m_decoderSlots[decoderIndex].m_recoveryBlocks[ir]; } + + qDebug() << "SDRdaemonFECBuffer::writeData:" + << " recovered block #" << blockIndex + << " i[0]: " << m_decoderSlots[decoderIndex].m_recoveryBlocks[ir].samples[0].i + << " q[0]: " << m_decoderSlots[decoderIndex].m_recoveryBlocks[ir].samples[0].q; } } } diff --git a/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.h b/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.h index 233926669..554cfc574 100644 --- a/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.h +++ b/plugins/samplesource/sdrdaemonfec/sdrdaemonfecbuffer.h @@ -92,10 +92,12 @@ public: // R/W operations void writeData(char *array, uint32_t length); //!< Write data into buffer. + void writeData0(char *array, uint32_t length); //!< Write data into buffer. uint8_t *readData(int32_t length); //!< Read data from buffer // meta data const MetaDataFEC& getCurrentMeta() const { return m_currentMeta; } + const MetaDataFEC& getOutputMeta() const { return m_outputMeta; } // stats int getCurNbBlocks() const { return m_curNbBlocks; } @@ -155,6 +157,7 @@ private: }; MetaDataFEC m_currentMeta; //!< Stored current meta data + MetaDataFEC m_outputMeta; //!< Meta data corresponding to currently served frame cm256_encoder_params m_paramsCM256; //!< CM256 decoder parameters block DecoderSlot m_decoderSlots[nbDecoderSlots]; //!< CM256 decoding control/buffer slots BufferFrame m_frames[nbDecoderSlots]; //!< Samples buffer