XnStatus Link12BitS2DParser::ParsePacketImpl(XnLinkFragmentation fragmentation,
const XnUInt8* pSrc,
const XnUInt8* pSrcEnd,
XnUInt8*& pDst,
const XnUInt8* pDstEnd)
{
XN_ASSERT(m_pShiftToDepth != NULL);
OniDepthPixel*& pDstPixel = reinterpret_cast<OniDepthPixel*&>(pDst);
const OniDepthPixel* pDstPixelEnd = reinterpret_cast<const OniDepthPixel*>(pDstEnd);
if ((fragmentation & XN_LINK_FRAG_BEGIN) != 0)
{
//Reset state for new frame
m_ContinuousBufferSize=0;
}
XnUInt32 bytesWritten = ProcessFramePacketChunk(pSrc,pDst,(XnUInt32)(pSrcEnd - pSrc));
pDstPixel += (OniDepthPixel)(bytesWritten/2); //progress pDst by the number of pixels (bytes divided by two)
if (pDstPixel > pDstPixelEnd) //Do we have enough room for this packet?
{
XN_ASSERT(FALSE);
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
return XN_STATUS_OK;
}
void XnFrameStreamProcessor::ProcessPacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnFrameStreamProcessor::ProcessPacketChunk");
// if first data from SOF packet
if (pHeader->nType == m_nTypeSOF && nDataOffset == 0)
{
if (!m_bAllowDoubleSOF || pHeader->nPacketID != (m_nLastSOFPacketID + 1))
{
m_nLastSOFPacketID = pHeader->nPacketID;
OnStartOfFrame(pHeader);
}
}
if (!m_bFrameCorrupted)
{
xnDumpWriteBuffer(m_InDump, pData, nDataSize);
ProcessFramePacketChunk(pHeader, pData, nDataOffset, nDataSize);
}
// if last data from EOF packet
if (pHeader->nType == m_nTypeEOF && (nDataOffset + nDataSize) == pHeader->nBufSize)
{
OnEndOfFrame(pHeader);
}
XN_PROFILING_END_SECTION
}
void XnFrameStreamProcessor::ProcessPacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize)
{
XN_PROFILING_START_SECTION("XnFrameStreamProcessor::ProcessPacketChunk");
// if first data from SOF packet
if (pHeader->nType == m_nTypeSOF && nDataOffset == 0)
{
if (!m_bAllowDoubleSOF || pHeader->nPacketID != (m_nLastSOFPacketID + 1))
{
XnUInt64 currOSTime;
xnOSGetTimeStamp(&currOSTime);
static const XnUInt32 halfSensorPeriod = 33/2; // in milliseconds
if(currOSTime - m_nLastSOFTimestamp > 1000 / XnSensor::ms_SoftVideoMode.fps - halfSensorPeriod)
{
xnLogVerbose(XN_MASK_SENSOR_PROTOCOL, "%s: Processing frame %d, t = %d (dt = %d)", m_csName, pHeader->nPacketID, int(currOSTime), int(currOSTime - m_nLastSOFTimestamp));
m_nLastSOFPacketID = pHeader->nPacketID;
OnStartOfFrame(pHeader);
m_nLastSOFTimestamp = currOSTime;
m_bProcessNextFrame = TRUE;
}
else
{
m_bProcessNextFrame = FALSE;
}
}
}
if(!m_bProcessNextFrame)
return;
if (!m_bFrameCorrupted)
{
xnDumpFileWriteBuffer(m_InDump, pData, nDataSize);
ProcessFramePacketChunk(pHeader, pData, nDataOffset, nDataSize);
}
// if last data from EOF packet
if (pHeader->nType == m_nTypeEOF && (nDataOffset + nDataSize) == pHeader->nBufSize)
{
OnEndOfFrame(pHeader);
}
XN_PROFILING_END_SECTION
}
