XnStatus XnDeviceFileWriter::WriteStream(XnStreamData* pStreamOutput)
{
XnStatus nRetVal = XN_STATUS_OK;
// start the timer on the first data written
nRetVal = StartTimer();
XN_IS_STATUS_OK(nRetVal);
// fill in timestamp if needed
if (pStreamOutput->nTimestamp == 0)
{
XnUInt64 nNow;
xnOSQueryTimer(m_Timer, &nNow);
if (!IsHighResTimestamps())
{
nNow /= 1000;
}
pStreamOutput->nTimestamp = nNow;
}
// and write it down (using base)
nRetVal = XnStreamWriterDevice::WriteStream(pStreamOutput);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
void XnDeviceFileReader::FrameDelay(XnUInt64 nTimestamp)
{
if (m_FrameDelay.GetValue() != TRUE)
return;
if (!IsHighResTimestamps())
nTimestamp *= 1000;
// first time
if (m_nReferenceTime == 0)
{
xnOSQueryTimer(m_FrameDelayTimer, &m_nReferenceTime);
m_nReferenceTimestamp = nTimestamp;
return;
}
// delay
XnUInt64 nNow;
xnOSQueryTimer(m_FrameDelayTimer, &nNow);
// check how much time has passed in the stream
XnUInt64 nStreamDiff;
if (nTimestamp < m_nReferenceTimestamp)
{
nStreamDiff = 0;
}
else
{
nStreamDiff = nTimestamp - m_nReferenceTimestamp;
}
// check how much time passed (for user)
XnUInt64 nClockDiff = nNow - m_nReferenceTime;
// update reference (so that frame delay will work with Pause / Resume)
m_nReferenceTime = nNow;
m_nReferenceTimestamp = nTimestamp;
// check if we need to wait
if (nClockDiff < nStreamDiff)
{
xnOSSleep(XnUInt32((nStreamDiff - nClockDiff) / 1000));
// take this time as a reference
xnOSQueryTimer(m_FrameDelayTimer, &m_nReferenceTime);
}
}
XnBool XnSensor::HasSynchedFrameArrived(const XnChar* strDepthStream, const XnChar* strImageStream)
{
// find both streams
XnDeviceStream* pDepth;
XnDeviceStream* pImage;
if (XN_STATUS_OK != FindStream(strDepthStream, &pDepth))
return FALSE;
if (XN_STATUS_OK != FindStream(strImageStream, &pImage))
return FALSE;
XnUInt32 nThreshold = XN_SENSOR_FRAME_SYNC_MAX_DIFF;
if (IsHighResTimestamps())
nThreshold *= 1000;
// wait for both to advance, and time difference to be less than threshold
XnInt32 nTimestampDiff = XnInt32(pDepth->GetLastTimestamp() - pImage->GetLastTimestamp());
XnBool bConditionMet = (
pDepth->IsNewDataAvailable() &&
pImage->IsNewDataAvailable() &&
(XnUInt32)abs(nTimestampDiff) <= nThreshold
);
if (xnLogIsDumpMaskEnabled(XN_DUMP_FRAME_SYNC))
{
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
xnDumpWriteString(m_FrameSyncDump, "%llu,%u,%llu,%u,%llu,%s\n",
nNow,
pDepth->IsNewDataAvailable(),
pDepth->GetLastTimestamp(),
pImage->IsNewDataAvailable(),
pImage->GetLastTimestamp(),
bConditionMet ? "OK" : "Waiting");
}
return bConditionMet;
}
