XnStatus Link24zYuv422Parser::Init()
{
m_dataFromPrevPacket = (XnUInt8*)xnOSMallocAligned(m_lineWidthBytes, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(m_dataFromPrevPacket);
if (m_transformToRGB)
{
m_tempYuvImage = (XnUInt8*)xnOSMallocAligned(m_rgbFrameSize, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(m_tempYuvImage);
}
return XN_STATUS_OK;
}
XN_DDK_API XnStatus XnStreamDataUpdateSize(XnStreamData* pStreamOutput, XnUInt32 nAllocSize)
{
XN_VALIDATE_INPUT_PTR(pStreamOutput);
// allocate new memory
void* pNew = xnOSMallocAligned(nAllocSize, XN_DEFAULT_MEM_ALIGN);
if (pNew == NULL)
return (XN_STATUS_ALLOC_FAILED);
// zero it
xnOSMemSet(pNew, 0, nAllocSize);
// free the buffer if it is allocated
XN_ALIGNED_FREE_AND_NULL(pStreamOutput->pData);
// and now set new buffer
pStreamOutput->pData = pNew;
// and size
pStreamOutput->pInternal->nAllocSize = nAllocSize;
pStreamOutput->pInternal->bAllocated = TRUE;
return XN_STATUS_OK;
}
XnStatus XnDeviceSensorProtocolDumpLastRawFrameImpl(XnDevicePrivateData* pDevicePrivateData, const XnChar* strType, const XnChar* strFileName)
{
XnStatus nRetVal = XN_STATUS_OK;
const XnChar* strName;
nRetVal = XnDeviceSensorProtocolFindStreamOfType(pDevicePrivateData, strType, &strName);
XN_IS_STATUS_OK(nRetVal);
XnUInt64 nMaxDataSize;
nRetVal = pDevicePrivateData->pSensor->GetProperty(strName, XN_STREAM_PROPERTY_REQUIRED_DATA_SIZE, &nMaxDataSize);
XN_IS_STATUS_OK(nRetVal);
XnDynamicSizeBuffer dsb;
dsb.nMaxSize = (XnUInt32)nMaxDataSize;
dsb.pData = xnOSMallocAligned((XnUInt32)nMaxDataSize, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(dsb.pData);
nRetVal = pDevicePrivateData->pSensor->GetProperty(strName, XN_STREAM_PROPERTY_LAST_RAW_FRAME, XN_PACK_GENERAL_BUFFER(dsb));
if (nRetVal != XN_STATUS_OK)
{
xnOSFreeAligned(dsb.pData);
return (nRetVal);
}
xnOSSaveFile(strFileName, dsb.pData, dsb.nDataSize);
xnOSFreeAligned(dsb.pData);
return (XN_STATUS_OK);
}
XnStatus LinkMsgEncoder::Init(XnUInt32 nMaxMsgSize, XnUInt16 nMaxPacketSize)
{
if (nMaxPacketSize == 0)
{
xnLogError(XN_MASK_LINK, "Got max packet size of 0 in link msg encoder init :(");
XN_ASSERT(FALSE);
return XN_STATUS_ERROR;
}
m_nMaxMsgSize = nMaxMsgSize;
m_nMaxPacketSize = nMaxPacketSize;
XnUInt16 nMaxPacketDataSize = m_nMaxPacketSize - sizeof(LinkPacketHeader);
m_nMaxNumPackets = m_nMaxMsgSize / nMaxPacketDataSize;
if (m_nMaxPacketSize % nMaxPacketDataSize != 0)
{
//We need one more packet for remainder of data
m_nMaxNumPackets++;
}
m_nBufferSize = m_nMaxNumPackets * m_nMaxPacketSize;
m_pOutputBuffer = reinterpret_cast<XnUInt8*>(xnOSMallocAligned(m_nBufferSize, XN_DEFAULT_MEM_ALIGN));
XN_VALIDATE_ALLOC_PTR(m_pOutputBuffer);
return XN_STATUS_OK;
}
void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) const
{
pFrame->frame.sensorType = sensor_type;
pFrame->frame.stride = video_mode.resolutionX*sizeof(uint16_t);
pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
pFrame->frame.croppingEnabled = FALSE;
pFrame->frame.dataSize = device->getDepthBufferSize();
pFrame->frame.data = xnOSMallocAligned(sizeof(uint16_t)*pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN);
if (pFrame->frame.data == NULL)
{
XN_ASSERT(FALSE);
return;
}
// copy stream buffer from freenect
uint16_t* _data = static_cast<uint16_t*>(data);
uint16_t* frame_data = static_cast<uint16_t*>(pFrame->frame.data);
if (mirroring)
{
for (unsigned int i = 0; i < pFrame->frame.dataSize; i++)
{
// find corresponding mirrored pixel
unsigned int row = i / video_mode.resolutionX;
unsigned int col = video_mode.resolutionX - (i % video_mode.resolutionX);
unsigned int target = (row * video_mode.resolutionX) + col;
// copy it to this pixel
frame_data[i] = _data[target];
}
}
else
std::copy(_data, _data+pFrame->frame.dataSize, frame_data);
}
XN_C_API XnStatus xnOSCreateSharedMemory(const XnChar* strName, XnUInt32 nSize, XnUInt32 nAccessFlags, XN_SHARED_MEMORY_HANDLE* phSharedMem)
{
void* pAddress = xnOSMallocAligned(nSize, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(pAddress);
*phSharedMem = (XN_SHARED_MEMORY_HANDLE)pAddress;
return (XN_STATUS_OK);
}
XnStatus XnSensorAudioStream::ReallocBuffer()
{
XnStatus nRetVal = XN_STATUS_OK;
if (m_buffer.pAudioBuffer == NULL)
{
// we allocate enough for 5 seconds of audio
XnUInt32 nSampleSize = 2 * 2; // 16-bit per channel (2 bytes) * max number of channels (2)
XnUInt32 nSamples = 48000 * 5; // max sample rate * number of seconds
XnUInt32 nMaxBufferSize = nSamples * nSampleSize;
// find min packet size (so we'll have max packet count)
XnUInt32 nMinPacketSize = XN_MIN(XN_SENSOR_PROTOCOL_AUDIO_PACKET_SIZE_BULK, XN_SENSOR_PROTOCOL_AUDIO_PACKET_SIZE_ISO);
XnUInt32 nMaxPacketCount = nMaxBufferSize / nMinPacketSize - 1;
nRetVal = RequiredSizeProperty().UnsafeUpdateValue(nMaxBufferSize);
XN_IS_STATUS_OK(nRetVal);
m_buffer.pAudioPacketsTimestamps = (XnUInt64*)xnOSMallocAligned(sizeof(XnUInt64) * nMaxPacketCount, XN_DEFAULT_MEM_ALIGN);
m_buffer.pAudioBuffer = (XnUInt8*)xnOSMallocAligned(nMaxBufferSize, XN_DEFAULT_MEM_ALIGN);
m_buffer.nAudioBufferSize = nMaxBufferSize;
}
// calculate current packet size
m_buffer.nAudioPacketSize = m_nOrigAudioPacketSize;
if (m_Helper.GetFirmwareVersion() >= XN_SENSOR_FW_VER_5_2 && GetNumberOfChannels() == 1)
{
m_buffer.nAudioPacketSize /= 2;
}
m_buffer.nAudioBufferNumOfPackets = m_buffer.nAudioBufferSize / m_buffer.nAudioPacketSize;
m_buffer.nAudioBufferSize = m_buffer.nAudioBufferNumOfPackets * m_buffer.nAudioPacketSize;
m_header.nPacketCount = m_buffer.nAudioBufferNumOfPackets;
m_header.nPacketSize = m_buffer.nAudioPacketSize;
// set read and write indices
m_buffer.nAudioReadIndex = 0;
m_buffer.nAudioWriteIndex = 0;
return (XN_STATUS_OK);
}
XnStatus MockGenerator::ResizeBuffer(XnUInt32 nIndex, XnUInt32 nNeededSize)
{
DataInfo& dataInfo = m_data[nIndex];
if (nNeededSize > dataInfo.nAllocatedSize)
{
xnOSFreeAligned(dataInfo.pData);
dataInfo.pData = xnOSMallocAligned(nNeededSize, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(dataInfo.pData);
dataInfo.nAllocatedSize = nNeededSize;
}
return (XN_STATUS_OK);
}
XnStatus SocketInConnection::Init(const XnChar* strIP, XnUInt16 nPort, XnUInt16 nMaxPacketSize)
{
XN_VALIDATE_INPUT_PTR(strIP);
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnOSStrCopy(m_strIP, strIP, sizeof(m_strIP));
XN_IS_STATUS_OK_LOG_ERROR("Copy IP", nRetVal);
m_nPort = nPort;
m_nMaxPacketSize = nMaxPacketSize;
m_nBufferSize = m_nMaxPacketSize * BUFFER_NUM_PACKETS;
m_pBuffer = reinterpret_cast<XnUInt8*>(xnOSMallocAligned(m_nBufferSize, XN_DEFAULT_MEM_ALIGN));
XN_VALIDATE_ALLOC_PTR(m_pBuffer);
nRetVal = xnOSCreateEvent(&m_hConnectEvent, FALSE);
XN_IS_STATUS_OK_LOG_ERROR("Create event", nRetVal);
xnLogVerbose(XN_MASK_LINK, "Event created for socket %u", m_nPort);
return XN_STATUS_OK;
}
void FreenectColorStream::populateFrame(void* data, OniDriverFrame* pFrame) const
{
pFrame->frame.sensorType = sensor_type;
pFrame->frame.stride = video_mode.resolutionX*3;
pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
pFrame->frame.croppingEnabled = FALSE;
pFrame->frame.dataSize = device->getVideoBufferSize();
pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN);
if (pFrame->frame.data == NULL)
{
XN_ASSERT(FALSE);
return;
}
// copy stream buffer from freenect
switch (video_mode.pixelFormat)
{
default:
printf("pixelFormat %s not supported by populateFrame\n", video_mode.pixelFormat);
return;
case ONI_PIXEL_FORMAT_RGB888:
unsigned char* _data = static_cast<unsigned char*>(data);
unsigned char* frame_data = static_cast<unsigned char*>(pFrame->frame.data);
if (mirroring)
{
for (unsigned int i = 0; i < pFrame->frame.dataSize; i += 3)
{
// find corresponding mirrored pixel
unsigned int pixel = i / 3;
unsigned int row = pixel / video_mode.resolutionX;
unsigned int col = video_mode.resolutionX - (pixel % video_mode.resolutionX);
unsigned int target = 3 * (row * video_mode.resolutionX + col);
// copy it to this pixel
frame_data[i] = _data[target];
frame_data[i+1] = _data[target+1];
frame_data[i+2] = _data[target+2];
}
}
else
std::copy(_data, _data+pFrame->frame.dataSize, frame_data);
return;
}
}
XnStatus XnDeviceSensorProtocolDumpLastRawFrameImpl(XnDevicePrivateData* pDevicePrivateData, const XnChar* strType, const XnChar* strFileName)
{
XnStatus nRetVal = XN_STATUS_OK;
const XnChar* strName;
nRetVal = XnDeviceSensorProtocolFindStreamOfType(pDevicePrivateData, strType, &strName);
XN_IS_STATUS_OK(nRetVal);
XnUInt64 nMaxDataSize;
nRetVal = pDevicePrivateData->pSensor->GetProperty(strName, XN_STREAM_PROPERTY_REQUIRED_DATA_SIZE, &nMaxDataSize);
XN_IS_STATUS_OK(nRetVal);
XnDynamicSizeBuffer dsb;
dsb.nMaxSize = (XnUInt32)nMaxDataSize;
dsb.pData = xnOSMallocAligned((XnUInt32)nMaxDataSize, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALLOC_PTR(dsb.pData);
nRetVal = pDevicePrivateData->pSensor->GetProperty(strName, XN_STREAM_PROPERTY_LAST_RAW_FRAME, XN_PACK_GENERAL_BUFFER(dsb));
if (nRetVal != XN_STATUS_OK)
{
xnOSFreeAligned(dsb.pData);
return (nRetVal);
}
// The real depth size is half of what's being reported because of special depth+shift memory packing format.
if (strType == XN_STREAM_TYPE_DEPTH)
{
dsb.nDataSize /= 2;
}
xnOSSaveFile(strFileName, dsb.pData, dsb.nDataSize);
xnOSFreeAligned(dsb.pData);
return (XN_STATUS_OK);
}
XnStatus XnDeviceSensorOpenInputThreads(XnDevicePrivateData* pDevicePrivateData, XnBool bOpen1, XnBool bOpen2, XnBool bOpen3)
{
if (bOpen2)
{
// Depth
pDevicePrivateData->pSpecificDepthUsb = (XnSpecificUsbDevice*)xnOSMallocAligned(sizeof(XnSpecificUsbDevice), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->pSpecificDepthUsb->pDevicePrivateData = pDevicePrivateData;
pDevicePrivateData->pSpecificDepthUsb->pUsbConnection = &pDevicePrivateData->SensorHandle.DepthConnection;
pDevicePrivateData->pSpecificDepthUsb->CurrState.State = XN_WAITING_FOR_CONFIGURATION;
if (pDevicePrivateData->pSpecificDepthUsb->pUsbConnection->bIsISO == TRUE)
{
if (pDevicePrivateData->pSensor->IsLowBandwidth())
{
pDevicePrivateData->pSpecificDepthUsb->nChunkReadBytes = XN_SENSOR_USB_DEPTH_BUFFER_SIZE_MULTIPLIER_LOWBAND_ISO * pDevicePrivateData->SensorHandle.DepthConnection.nMaxPacketSize;
}
else
{
pDevicePrivateData->pSpecificDepthUsb->nChunkReadBytes = XN_SENSOR_USB_DEPTH_BUFFER_SIZE_MULTIPLIER_ISO * pDevicePrivateData->SensorHandle.DepthConnection.nMaxPacketSize;
}
pDevicePrivateData->pSpecificDepthUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_ISO;
}
else
{
pDevicePrivateData->pSpecificDepthUsb->nChunkReadBytes = XN_SENSOR_USB_DEPTH_BUFFER_SIZE_MULTIPLIER_BULK * pDevicePrivateData->SensorHandle.DepthConnection.nMaxPacketSize;
pDevicePrivateData->pSpecificDepthUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_BULK;
}
pDevicePrivateData->pSpecificDepthUsb->nIgnoreBytes = (pDevicePrivateData->FWInfo.nFWVer >= XN_SENSOR_FW_VER_5_0) ? 0 : pDevicePrivateData->pSpecificDepthUsb->nChunkReadBytes;
}
if (bOpen1)
{
// Image
pDevicePrivateData->pSpecificImageUsb = (XnSpecificUsbDevice*)xnOSMallocAligned(sizeof(XnSpecificUsbDevice), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->pSpecificImageUsb->pDevicePrivateData = pDevicePrivateData;
pDevicePrivateData->pSpecificImageUsb->pUsbConnection = &pDevicePrivateData->SensorHandle.ImageConnection;
pDevicePrivateData->pSpecificImageUsb->CurrState.State = XN_WAITING_FOR_CONFIGURATION;
if (pDevicePrivateData->pSpecificImageUsb->pUsbConnection->bIsISO == TRUE)
{
if (pDevicePrivateData->pSensor->IsLowBandwidth())
{
pDevicePrivateData->pSpecificImageUsb->nChunkReadBytes = XN_SENSOR_USB_IMAGE_BUFFER_SIZE_MULTIPLIER_LOWBAND_ISO * pDevicePrivateData->SensorHandle.ImageConnection.nMaxPacketSize;
}
else
{
pDevicePrivateData->pSpecificImageUsb->nChunkReadBytes = XN_SENSOR_USB_IMAGE_BUFFER_SIZE_MULTIPLIER_ISO * pDevicePrivateData->SensorHandle.ImageConnection.nMaxPacketSize;
}
pDevicePrivateData->pSpecificImageUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_ISO;
}
else
{
pDevicePrivateData->pSpecificImageUsb->nChunkReadBytes = XN_SENSOR_USB_IMAGE_BUFFER_SIZE_MULTIPLIER_BULK * pDevicePrivateData->SensorHandle.ImageConnection.nMaxPacketSize;
pDevicePrivateData->pSpecificImageUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_BULK;
}
pDevicePrivateData->pSpecificImageUsb->nIgnoreBytes = (pDevicePrivateData->FWInfo.nFWVer >= XN_SENSOR_FW_VER_5_0) ? 0 : pDevicePrivateData->pSpecificImageUsb->nChunkReadBytes;
}
// Misc
if (bOpen3 && pDevicePrivateData->pSensor->IsMiscSupported())
{
pDevicePrivateData->pSpecificMiscUsb = (XnSpecificUsbDevice*)xnOSMallocAligned(sizeof(XnSpecificUsbDevice), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->pSpecificMiscUsb->pDevicePrivateData = pDevicePrivateData;
pDevicePrivateData->pSpecificMiscUsb->pUsbConnection = &pDevicePrivateData->SensorHandle.MiscConnection;
pDevicePrivateData->pSpecificMiscUsb->CurrState.State = XN_WAITING_FOR_CONFIGURATION;
if (pDevicePrivateData->pSpecificMiscUsb->pUsbConnection->bIsISO == TRUE)
{
if (pDevicePrivateData->pSensor->IsLowBandwidth())
{
pDevicePrivateData->pSpecificMiscUsb->nChunkReadBytes = XN_SENSOR_USB_MISC_BUFFER_SIZE_MULTIPLIER_LOWBAND_ISO * pDevicePrivateData->SensorHandle.MiscConnection.nMaxPacketSize;
}
else
{
pDevicePrivateData->pSpecificMiscUsb->nChunkReadBytes = XN_SENSOR_USB_MISC_BUFFER_SIZE_MULTIPLIER_ISO * pDevicePrivateData->SensorHandle.MiscConnection.nMaxPacketSize;
}
pDevicePrivateData->pSpecificMiscUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_ISO;
}
else
{
pDevicePrivateData->pSpecificMiscUsb->nChunkReadBytes = XN_SENSOR_USB_MISC_BUFFER_SIZE_MULTIPLIER_BULK * pDevicePrivateData->SensorHandle.MiscConnection.nMaxPacketSize;
pDevicePrivateData->pSpecificMiscUsb->nTimeout = XN_SENSOR_READ_THREAD_TIMEOUT_BULK;
}
pDevicePrivateData->pSpecificMiscUsb->nIgnoreBytes = (pDevicePrivateData->FWInfo.nFWVer >= XN_SENSOR_FW_VER_5_0) ? 0 : pDevicePrivateData->pSpecificMiscUsb->nChunkReadBytes;
}
// Switch depth & image EPs for older FWs
if (pDevicePrivateData->FWInfo.nFWVer <= XN_SENSOR_FW_VER_5_1)
{
XnSpecificUsbDevice* pTempUsbDevice = pDevicePrivateData->pSpecificDepthUsb;
pDevicePrivateData->pSpecificDepthUsb = pDevicePrivateData->pSpecificImageUsb;
pDevicePrivateData->pSpecificImageUsb = pTempUsbDevice;
}
return XN_STATUS_OK;
}
