//---------------------------------------------------------------------------
// PNP Functions
//---------------------------------------------------------------------------
DWORD __stdcall DevDetectThread(LPVOID Arg)
{
char className[MAX_PATH];
sprintf_s(className, "xnUsbDeviceDetector%x", &g_xnUsbhModule);
g_xnUsbhDevDetectWnd = CreateWindow(className, "", WS_POPUP, 0, 0, 0, 0, NULL, NULL, NULL, NULL);
SetEvent((HANDLE)Arg);
if(g_xnUsbhDevDetectWnd)
{
MSG msg;
while (g_bUsbDevDetectShoudRun)
{
if (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
{
DispatchMessage(&msg);
}
else
{
xnOSSleep(50);
}
}
DestroyWindow(g_xnUsbhDevDetectWnd);
}
return 0;
}
void Mainloop()
{
int frameId = 1;
int xdir = 1;
int ydir = 1;
struct {int x, y;} center = {0,0};
while (m_running)
{
// printf("Tick");
OniFrame* pFrame = getServices().acquireFrame();
if (pFrame == NULL) {printf("Didn't get frame...\n"); continue;}
// Fill frame
xnOSMemSet(pFrame->data, 0, pFrame->dataSize);
OniDepthPixel* pDepth = (OniDepthPixel*)pFrame->data;
for (int y1 = XN_MAX(center.y-10, 0); y1 < XN_MIN(center.y+10, OZ_RESOLUTION_Y); ++y1)
for (int x1 = XN_MAX(center.x-10, 0); x1 < XN_MIN(center.x+10, OZ_RESOLUTION_X); ++x1)
if ((x1-center.x)*(x1-center.x)+(y1-center.y)*(y1-center.y) < 70)
pDepth[singleRes(x1, y1)] = OniDepthPixel(1000+(x1-y1)*3);
// pDepth[singleRes(center.x, center.y)] = 1000;
center.x += xdir;
center.y += ydir;
if (center.x < abs(xdir) || center.x > OZ_RESOLUTION_X-1-abs(xdir)) xdir*=-1;
if (center.y < abs(ydir) || center.y > OZ_RESOLUTION_Y-1-abs(ydir)) ydir*=-1;
for (int i = 0; i < OZ_RESOLUTION_X; ++i) pDepth[i] = 2000;
pDepth[0] = 2000;
// Fill metadata
pFrame->frameIndex = frameId;
pFrame->videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
pFrame->videoMode.resolutionX = OZ_RESOLUTION_X;
pFrame->videoMode.resolutionY = OZ_RESOLUTION_Y;
pFrame->videoMode.fps = 30;
pFrame->width = OZ_RESOLUTION_X;
pFrame->height = OZ_RESOLUTION_Y;
pFrame->cropOriginX = pFrame->cropOriginY = 0;
pFrame->croppingEnabled = FALSE;
pFrame->sensorType = ONI_SENSOR_DEPTH;
pFrame->stride = OZ_RESOLUTION_X*sizeof(OniDepthPixel);
pFrame->timestamp = frameId*33000;
raiseNewFrame(pFrame);
getServices().releaseFrame(pFrame);
frameId++;
xnOSSleep(33);
}
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XN_THREAD_PROC xnProfilingThread(XN_THREAD_PARAM /*pThreadParam*/)
{
XnChar csReport[4096];
int nReportChars;
XnUInt64 nLastTime;
xnOSGetHighResTimeStamp(&nLastTime);
while (!g_ProfilingData.bKillThread)
{
xnOSSleep(g_ProfilingData.nProfilingInterval);
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
// print profiled sections
nReportChars = 0;
nReportChars += sprintf(csReport + nReportChars, "Profiling Report:\n");
nReportChars += sprintf(csReport + nReportChars, "%-*s %-5s %-6s %-9s %-7s\n", (int)g_ProfilingData.nMaxSectionName, "TaskName", "Times", "% Time", "TotalTime", "AvgTime");
nReportChars += sprintf(csReport + nReportChars, "%-*s %-5s %-6s %-9s %-7s\n", (int)g_ProfilingData.nMaxSectionName, "========", "=====", "======", "=========", "=======");
XnUInt64 nTotalTime = 0;
for (XnUInt32 i = 0; i < g_ProfilingData.nSectionCount; ++i)
{
XnProfiledSection* pSection = &g_ProfilingData.aSections[i];
XnUInt64 nAvgTime = 0;
XnDouble dCPUPercentage = ((XnDouble)pSection->nTotalTime) / (nNow - nLastTime) * 100.0;
if (pSection->nTimesExecuted != 0)
{
nAvgTime = pSection->nTotalTime / pSection->nTimesExecuted;
}
nReportChars += sprintf(csReport + nReportChars, "%-*s %5u %6.2f %9llu %7llu\n", (int)g_ProfilingData.nMaxSectionName,
pSection->csName, pSection->nTimesExecuted, dCPUPercentage, pSection->nTotalTime, nAvgTime);
if (pSection->nIndentation == 0)
nTotalTime += pSection->nTotalTime;
// clear accumulated data
pSection->nTotalTime = 0;
pSection->nTimesExecuted = 0;
}
// print total
XnDouble dCPUPercentage = ((XnDouble)nTotalTime) / (nNow - nLastTime) * 100.0;
nReportChars += sprintf(csReport + nReportChars, "%-*s %5s %6.2f %9llu %7s\n",
(int)g_ProfilingData.nMaxSectionName, "*** Total ***", "-", dCPUPercentage, nTotalTime, "-");
xnLogVerbose(XN_MASK_PROFILING, "%s", csReport);
nLastTime = nNow;
}
XN_THREAD_PROC_RETURN(XN_STATUS_OK);
}
void XnVMessageListener::MainLoop()
{
SetCurrentThread();
while (!m_bInternalThreadKill)
{
Run();
xnOSSleep(10);
}
m_bInternalThreadAlive = false;
} // XnVMessageListener::MainLoop
XN_THREAD_PROC DepthPanasonic::SchedulerThread( void* pCookie )
{
DepthPanasonic* pThis = (DepthPanasonic*)pCookie;
while (pThis->m_bGenerating)
{
// wait 33 ms (to produce 30 FPS)
xnOSSleep(1000000/SUPPORTED_FPS/1000);
pThis->OnNewFrame();
}
XN_THREAD_PROC_RETURN(0);
}
void VideoStream::newFrameThreadMainloop()
{
XnStatus rc = XN_STATUS_OK;
// Wait on frame
while (m_running)
{
rc = xnOSWaitEvent(m_newFrameInternalEvent, XN_WAIT_INFINITE);
if ((rc == XN_STATUS_OK) && m_running)
{
m_newFrameEvent.Raise();
// HACK: To avoid starvation of other threads.
xnOSSleep(1);
}
}
}
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);
}
}
XnStatus XnDeviceSensorConfigureVersion(XnDevicePrivateData* pDevicePrivateData)
{
XnStatus nRetVal = XN_STATUS_OK;
// GetVersion is exactly the same in all versions, except a change that was made in version 5.1.
// so, we'll start with that, and if doesn't work we'll try previous protocols
XnHostProtocolUsbCore usb = XN_USB_CORE_JANGO;
nRetVal = XnHostProtocolInitFWParams(pDevicePrivateData, 5, 1, 0, usb);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnHostProtocolGetVersion(pDevicePrivateData, pDevicePrivateData->Version);
// Strange bug: sometimes, when sending first command to device, no reply is received, so try again
if (nRetVal == XN_STATUS_USB_TRANSFER_TIMEOUT)
{
xnOSSleep(2000);
nRetVal = XnHostProtocolGetVersion(pDevicePrivateData, pDevicePrivateData->Version);
}
// if command failed for any reason, try again with older protocol
if (nRetVal != XN_STATUS_OK)
{
nRetVal = XnHostProtocolInitFWParams(pDevicePrivateData, 5, 0, 0, usb);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnHostProtocolGetVersion(pDevicePrivateData, pDevicePrivateData->Version);
}
// if it still fails, give up
XN_IS_STATUS_OK(nRetVal);
// check which usb core is used (don't check error code. If this fails, assume JANGO
if (XN_STATUS_OK != XnHostProtocolGetUsbCoreType(pDevicePrivateData, usb))
{
usb = XN_USB_CORE_JANGO;
}
// Now that we have the actual version, configure protocol accordingly
nRetVal = XnHostProtocolInitFWParams(pDevicePrivateData, pDevicePrivateData->Version.nMajor, pDevicePrivateData->Version.nMinor, pDevicePrivateData->Version.nBuild, usb);
XN_IS_STATUS_OK(nRetVal);
pDevicePrivateData->HWInfo.nHWVer = pDevicePrivateData->Version.HWVer;
pDevicePrivateData->ChipInfo.nChipVer = pDevicePrivateData->Version.ChipVer;
return (XN_STATUS_OK);
}
XnStatus ClientUSBControlEndpoint::Receive(void* pData, XnUInt32& nSize)
{
XnUInt32 nBufferSize = nSize;
XnStatus nRetVal = XN_STATUS_OK;
// Workaround devices bug: in some devices (Lena for example), one cannot receive
// immediately after send. The in-request should arrive AFTER the device has
// finished reading the entire out-request and its data, and clear the state.
// otherwise, device stalls.
xnOSSleep(m_nPreControlReceiveSleep);
nRetVal = xnUSBReceiveControl(m_hUSBDevice, XN_USB_CONTROL_TYPE_VENDOR, 0, 0, 0,
reinterpret_cast<XnUChar*>(pData), nBufferSize, &nSize, RECEIVE_TIMEOUT);
XN_IS_STATUS_OK_LOG_ERROR("Receive buffer from USB", nRetVal);
return XN_STATUS_OK;
}
void PlayerDevice::SleepToTimestamp(XnUInt64 nTimeStamp)
{
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
m_cs.Lock();
XnBool bHasTimeReference = TRUE;
if (!m_bHasTimeReference /*&& (nTimeStamp <= m_nStartTimestamp)*/)
{
m_nStartTimestamp = nTimeStamp;
m_nStartTime = nNow;
m_bHasTimeReference = TRUE;
bHasTimeReference = FALSE;
}
m_cs.Unlock();
if (bHasTimeReference && (m_dPlaybackSpeed > 0.0f))
{
// check this data timestamp compared to when we started
XnInt64 nTimestampDiff = nTimeStamp - m_nStartTimestamp;
// in some recordings, frames are not ordered by timestamp. Make sure this does not break the mechanism
if (nTimestampDiff > 0)
{
XnInt64 nTimeDiff = nNow - m_nStartTime;
// check if we need to wait some time
XnInt64 nRequestedTimeDiff = (XnInt64)(nTimestampDiff / m_dPlaybackSpeed);
if (nTimeDiff < nRequestedTimeDiff)
{
XnUInt32 nSleep = XnUInt32((nRequestedTimeDiff - nTimeDiff)/1000);
nSleep = XN_MIN(nSleep, XN_PLAYBACK_SPEED_SANITY_SLEEP);
xnOSSleep(nSleep);
}
// update reference to current frame (this will handle cases in which application
// stopped reading frames and continued after a while)
m_nStartTimestamp = nTimeStamp;
xnOSGetHighResTimeStamp(&m_nStartTime);
}
}
}
void PrimeClient::Shutdown()
{
if (m_bInitialized)
{
for (XnUInt32 nEndpointID = 0; nEndpointID < m_inputDataEndpoints.GetSize(); nEndpointID++)
{
m_inputDataEndpoints[nEndpointID].Shutdown();
}
m_outputDataEndpoint.Shutdown();
//First shutdown stream managers before control endpoint, because they might need to send a StopStreaming command
m_linkOutputStreamsMgr.Shutdown();
m_linkInputStreamsMgr.Shutdown();
m_linkControlEndpoint.Shutdown();
xnOSSleep(200); //TODO: Get rid of this once we have a disconnection command
m_pConnectionFactory->Shutdown();
XN_DELETE(m_pConnectionFactory);
m_pConnectionFactory = NULL;
m_bInitialized = FALSE;
}
}
XnStatus XnDeviceSensorConfigureVersion(XnDevicePrivateData* pDevicePrivateData)
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = XnHostProtocolGetVersion(pDevicePrivateData, pDevicePrivateData->Version);
// Strange bug: sometimes, when sending first command to device, no reply is received, so try again
if (nRetVal == XN_STATUS_USB_TRANSFER_TIMEOUT)
{
xnOSSleep(2000);
nRetVal = XnHostProtocolGetVersion(pDevicePrivateData, pDevicePrivateData->Version);
}
if (nRetVal != XN_STATUS_OK)
{
return nRetVal;
}
// Sensor HW Version is always 1.0 now...
pDevicePrivateData->SensorInfo.nSensorVer = XN_SENSOR_VER_2_0;
return (XN_STATUS_OK);
}
XnStatus PlayerImpl::SetNodeNewData(const XnChar* strNodeName, XnUInt64 nTimeStamp, XnUInt32 nFrame, const void* pData, XnUInt32 nSize)
{
XnStatus nRetVal = XN_STATUS_OK;
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
if (!m_bHasTimeReference)
{
m_nStartTimestamp = nTimeStamp;
m_nStartTime = nNow;
m_bHasTimeReference = TRUE;
}
if (m_dPlaybackSpeed != XN_PLAYBACK_SPEED_FASTEST)
{
// check this data timestamp compared to when we started
XnInt64 nTimestampDiff = nTimeStamp - m_nStartTimestamp;
XnInt64 nTimeDiff = nNow - m_nStartTime;
// check if we need to wait some time
XnInt64 nRequestedTimeDiff = (XnInt64)(nTimestampDiff / m_dPlaybackSpeed);
if (nTimeDiff < nRequestedTimeDiff)
{
xnOSSleep(XnUInt32((nRequestedTimeDiff - nTimeDiff)/1000));
}
// update reference to current frame (this will handle cases in which application
// stopped reading frames and continued after a while)
m_nStartTimestamp = nTimeStamp;
xnOSGetHighResTimeStamp(&m_nStartTime);
}
PlayedNodeInfo playedNode;
nRetVal = m_playedNodes.Get(strNodeName, playedNode);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnLockedNodeStartChanges(playedNode.hNode, playedNode.hLock);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnSetIntProperty(playedNode.hNode, XN_PROP_TIMESTAMP, nTimeStamp);
if (nRetVal != XN_STATUS_OK)
{
xnLockedNodeEndChanges(playedNode.hNode, playedNode.hLock);
return (nRetVal);
}
nRetVal = xnSetIntProperty(playedNode.hNode, XN_PROP_FRAME_ID, nFrame);
if (nRetVal != XN_STATUS_OK)
{
xnLockedNodeEndChanges(playedNode.hNode, playedNode.hLock);
return (nRetVal);
}
nRetVal = xnSetGeneralProperty(playedNode.hNode, XN_PROP_NEWDATA, nSize, pData);
if (nRetVal != XN_STATUS_OK)
{
xnLockedNodeEndChanges(playedNode.hNode, playedNode.hLock);
return (nRetVal);
}
nRetVal = xnLockedNodeEndChanges(playedNode.hNode, playedNode.hLock);
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus XnSensorFirmware::Init(XnBool bReset)
{
XnStatus nRetVal = XN_STATUS_OK;
// check current mode
XnUInt16 nMode;
nRetVal = XnHostProtocolGetMode(m_pDevicePrivateData, nMode);
XN_IS_STATUS_OK(nRetVal);
if (bReset)
{
// check if safe mode
if (nMode == XN_HOST_PROTOCOL_MODE_SAFE_MODE)
{
XN_LOG_WARNING_RETURN(XN_STATUS_DEVICE_SAFE_MODE, XN_MASK_DEVICE_SENSOR, "Device is in safe mode. Cannot start any stream!");
}
// check if device is alive
XnUInt32 nCounter = 5;
while (nCounter)
{
nRetVal = XnHostProtocolKeepAlive(m_pDevicePrivateData);
if (nRetVal != XN_STATUS_OK)
{
nCounter--;
}
else
nCounter = 0;
}
if (nRetVal != XN_STATUS_OK)
{
printf("Keep alive failed!\n");
return nRetVal;
}
// perform a soft reset (to start clean)
nRetVal = XnHostProtocolReset(m_pDevicePrivateData, XN_RESET_TYPE_SOFT_FIRST);
if (nRetVal != XN_STATUS_OK)
{
printf("Couldn't reset the device!\n");
return nRetVal;
}
// wait for sensor to recover from reset
xnOSSleep(m_pDevicePrivateData->FWInfo.nUSBDelaySoftReset);
// send keep alive again to see sensor is up
nCounter = 10;
while (nCounter)
{
nRetVal = XnHostProtocolKeepAlive(m_pDevicePrivateData);
if (nRetVal != XN_STATUS_OK)
{
nCounter--;
xnOSSleep(10);
}
else
break;
}
if (nCounter == 0)
{
printf("10 keep alives is too much - stopping\n");
return nRetVal;
}
nRetVal = XnHostProtocolGetMode(m_pDevicePrivateData, nMode);
XN_IS_STATUS_OK(nRetVal);
// check if safe mode
if (nMode == XN_HOST_PROTOCOL_MODE_SAFE_MODE)
{
XN_LOG_WARNING_RETURN(XN_STATUS_DEVICE_SAFE_MODE, XN_MASK_DEVICE_SENSOR, "Device is in safe mode. Cannot start any stream!");
}
}
nRetVal = m_Params.Init();
XN_IS_STATUS_OK(nRetVal);
if (nMode == XN_HOST_PROTOCOL_MODE_PS)
{
nRetVal = m_Params.UpdateAllProperties();
XN_IS_STATUS_OK(nRetVal);
}
nRetVal = m_Streams.Init();
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnSensorFirmware::Init(XnBool bReset, XnBool bLeanInit)
{
XnStatus nRetVal = XN_STATUS_OK;
// check current mode
XnUInt16 nMode;
nRetVal = XnHostProtocolGetMode(m_pDevicePrivateData, nMode);
XN_IS_STATUS_OK(nRetVal);
if (bReset)
{
// check if safe mode
if (nMode == XN_HOST_PROTOCOL_MODE_SAFE_MODE)
{
XN_LOG_WARNING_RETURN(XN_STATUS_DEVICE_SAFE_MODE, XN_MASK_DEVICE_SENSOR, "Device is in safe mode. Cannot start any stream!");
}
// check if device is alive
XnUInt32 nCounter = 5;
while (nCounter)
{
nRetVal = XnHostProtocolKeepAlive(m_pDevicePrivateData);
if (nRetVal != XN_STATUS_OK)
{
nCounter--;
}
else
nCounter = 0;
}
if (nRetVal != XN_STATUS_OK)
{
printf("Keep alive failed!\n");
return nRetVal;
}
// perform a soft reset (to start clean)
nRetVal = XnHostProtocolReset(m_pDevicePrivateData, XN_RESET_TYPE_SOFT_FIRST);
if (nRetVal != XN_STATUS_OK)
{
printf("Couldn't reset the device!\n");
return nRetVal;
}
// wait for sensor to recover from reset
xnOSSleep(m_pDevicePrivateData->FWInfo.nUSBDelaySoftReset);
// send keep alive again to see sensor is up
nCounter = 10;
while (nCounter)
{
nRetVal = XnHostProtocolKeepAlive(m_pDevicePrivateData);
if (nRetVal != XN_STATUS_OK)
{
nCounter--;
xnOSSleep(10);
}
else
break;
}
if (nCounter == 0)
{
printf("10 keep alives is too much - stopping\n");
return nRetVal;
}
nRetVal = XnHostProtocolGetMode(m_pDevicePrivateData, nMode);
XN_IS_STATUS_OK(nRetVal);
// check if safe mode
if (nMode == XN_HOST_PROTOCOL_MODE_SAFE_MODE)
{
XN_LOG_WARNING_RETURN(XN_STATUS_DEVICE_SAFE_MODE, XN_MASK_DEVICE_SENSOR, "Device is in safe mode. Cannot start any stream!");
}
}
if (!bLeanInit)
{
nRetVal = m_FixedParams.Init();
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_Params.Init();
XN_IS_STATUS_OK(nRetVal);
if (nMode == XN_HOST_PROTOCOL_MODE_PS)
{
nRetVal = m_Params.UpdateAllProperties();
XN_IS_STATUS_OK(nRetVal);
}
// Check if image is supported
if (m_pInfo->bGetImageCmosTypeSupported)
{
m_pInfo->bImageSupported = (m_FixedParams.GetImageCmosType() != 0);
}
else
{
// This is an ugly patch to find out if this sensor has an image CMOS. It will be fixed
// in future firmwares so we can just ask.
XnUInt16 nLines;
nRetVal = XnHostProtocolGetCmosBlanking(m_pDevicePrivateData, XN_CMOS_TYPE_IMAGE, &nLines);
m_pInfo->bImageSupported = (nRetVal == XN_STATUS_OK && nLines > 0);
}
nRetVal = m_Streams.Init();
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
XnStatus PS1200Device::UsbTest(XnUInt32 nSeconds, XnUInt32& endpointsCount, XnUsbTestEndpointResult* endpoints)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::ClientUSBConnectionFactory* pConnFactory = GetConnectionFactory();
if (m_linkInputStreamsMgr.HasStreams())
{
xnLogWarning(XN_MASK_PS1200_DEVICE, "Can't start USB test when other streams exists!");
return XN_STATUS_ERROR;
}
XnUInt16 nNumEndpoints = pConnFactory->GetNumInputDataConnections();
if (nNumEndpoints > endpointsCount)
{
xnLogWarning(XN_MASK_PS1200_DEVICE, "Endpoints array is too small");
return XN_STATUS_BAD_PARAM;
}
xn::IAsyncInputConnection* aEndpoints[20];
UsbEndpointTester aTesters[20];
for (int i = 0; i < nNumEndpoints; ++i)
{
nRetVal = pConnFactory->CreateInputDataConnection((XnUInt16)i, aEndpoints[i]);
if (nRetVal != XN_STATUS_OK)
{
for (int j = 0; j < i; ++j)
{
XN_DELETE(aEndpoints[j]);
}
return nRetVal;
}
aTesters[i].Reset();
aTesters[i].m_nEP = i;
aEndpoints[i]->SetDataDestination(&aTesters[i]);
aEndpoints[i]->Connect();
}
nRetVal = m_linkControlEndpoint.StartUsbTest();
if (nRetVal != XN_STATUS_OK)
{
for (int i = 0; i < nNumEndpoints; ++i)
{
XN_DELETE(aEndpoints[i]);
}
return nRetVal;
}
// let the test run
xnOSSleep(nSeconds*1000);
nRetVal = m_linkControlEndpoint.StopUsbTest();
if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_PS1200_DEVICE, "Failed to stop USB test!");
XN_ASSERT(FALSE);
}
for (int i = 0; i < nNumEndpoints; ++i)
{
XN_DELETE(aEndpoints[i]);
endpoints[i].averageBytesPerSecond = aTesters[i].m_nTotalBytes / (XnDouble)nSeconds;
endpoints[i].lostPackets = aTesters[i].m_nLostPackets;
}
endpointsCount = nNumEndpoints;
return (XN_STATUS_OK);
}
void Mainloop()
{
int frameId = 1;
int xdir = -3;
int ydir = 1;
struct {int x, y;} center = {160,120};
while (m_running)
{
xnOSSleep(33);
// printf("Tick");
OniFrame* pFrame = getServices().acquireFrame();
if (pFrame == NULL) {printf("Didn't get frame...\n"); continue;}
// Fill frame
xnOSMemSet(pFrame->data, 0, pFrame->dataSize);
OniRGB888Pixel* pImage = (OniRGB888Pixel*)pFrame->data;
for (int y = XN_MAX(center.y-10, 0); y < XN_MIN(center.y+10, OZ_RESOLUTION_Y); ++y)
for (int x = XN_MAX(center.x-10, 0); x < XN_MIN(center.x+10, OZ_RESOLUTION_X); ++x)
if ((x-center.x)*(x-center.x)+(y-center.y)*(y-center.y) < 70)
{
pImage[singleRes(x, y)].r = (char)(255*(x/(double)OZ_RESOLUTION_X));
pImage[singleRes(x, y)].g = (char)(255*(y/(double)OZ_RESOLUTION_Y));
pImage[singleRes(x, y)].b = (char)(255*((OZ_RESOLUTION_X-x)/(double)OZ_RESOLUTION_X));
}
// pImage[singleRes(center.x, center.y)].r = 255;
center.x += xdir;
center.y += ydir;
if (center.x < abs(xdir) || center.x > OZ_RESOLUTION_X-1-abs(xdir)) xdir*=-1;
if (center.y < abs(ydir) || center.y > OZ_RESOLUTION_Y-1-abs(ydir)) ydir*=-1;
pImage[0].b = (unsigned char)255;
// for (int y = 0; y < OZ_RESOLUTION_Y; ++y)
// {
// pDepth[y*OZ_RESOLUTION_X+(OZ_RESOLUTION_Y-y)] = pDepth[y*OZ_RESOLUTION_X+(y)] = 500+y;
// }
// Fill metadata
pFrame->frameIndex = frameId;
pFrame->videoMode.pixelFormat = ONI_PIXEL_FORMAT_RGB888;
pFrame->videoMode.resolutionX = OZ_RESOLUTION_X;
pFrame->videoMode.resolutionY = OZ_RESOLUTION_Y;
pFrame->videoMode.fps = 30;
pFrame->width = OZ_RESOLUTION_X;
pFrame->height = OZ_RESOLUTION_Y;
pFrame->cropOriginX = pFrame->cropOriginY = 0;
pFrame->croppingEnabled = FALSE;
pFrame->sensorType = ONI_SENSOR_COLOR;
pFrame->stride = OZ_RESOLUTION_X*3;
pFrame->timestamp = frameId*33000;
raiseNewFrame(pFrame);
getServices().releaseFrame(pFrame);
frameId++;
}
}
void PlayerDevice::MainLoop()
{
m_running = true;
while (m_running)
{
// Process data only when at least one of the streams within has started
bool waitForStreamStart = true;
for (StreamList::Iterator iter = m_streams.Begin(); iter != m_streams.End(); iter++)
{
PlayerStream* pStream = *iter;
if (pStream->isStreamStarted())
{
waitForStreamStart = false;
break;
}
}
if (waitForStreamStart)
{
xnOSSleep(10);
continue;
}
if (m_isSeeking)
{
// Set playback speed to maximum.
double playbackSpeed = m_dPlaybackSpeed;
m_dPlaybackSpeed = XN_PLAYBACK_SPEED_FASTEST;
// Seek the frame ID for first source (seek to (frame ID-1) so next read frame is frameId).
PlayerSource* pSource = m_seek.pStream->GetSource();
XnStatus xnrc = m_player.SeekToFrame(pSource->GetNodeName(), m_seek.frameId, XN_PLAYER_SEEK_SET);
if (xnrc != XN_STATUS_OK)
{
// Failure to seek.
m_isSeeking = FALSE;
continue;
}
// Return playback speed to normal.
m_dPlaybackSpeed = playbackSpeed;
// Reset the wait events.
m_readyForDataInternalEvent.Reset();
m_manualTriggerInternalEvent.Reset();
// Reset the time reference.
m_bHasTimeReference = FALSE;
// Raise the seek complete event.
m_SeekCompleteInternalEvent.Set();
// Mark the seeking flag as false.
m_isSeeking = FALSE;
}
else
{
// Read the next frame (delay between frames must be dealt with in the OnNodeNewData callback).
m_player.ReadNext();
}
}
}
