XnStatus DepthPanasonic::Init()
{
// Aloca o mapa de profundidade
m_pDepthMap = new XnDepthPixel[SUPPORTED_X_RES * SUPPORTED_Y_RES];
if (m_pDepthMap == NULL)
{
return XN_STATUS_ALLOC_FAILED;
}
// Inicializa a câmera
if (InitImageDriver() != 0) {
return XN_STATUS_DEVICE_NOT_CONNECTED;
}
// Define a taxa de captura
if (Speedmode () != SUPPORTED_FPS){
ChangeSpeed (SUPPORTED_FPS);
}
// Define a frequência da luz (evitar interferênia com outras)
if (Freqmode () != 1){
ChangeFreq (1);
}
// Acorda a câmera
if (Sleepmode () == 0){
ChangeSleep (1);
}
// Aloca memória para os vetores retornados pela câmera (160*120*2 bytes = 38400 bytes)
kdat = (unsigned short*) malloc(38400);
ndat = (unsigned short*) malloc(38400);
// Inicialização das matrizes que convertem os valores de profundidade para mm (e vice versa)
XN_VALIDATE_INPUT_PTR(&m_ShiftToDepthTables);
XN_VALIDATE_INPUT_PTR(&m_s2dConfig);
XN_VALIDATE_ALIGNED_CALLOC(m_ShiftToDepthTables.pShiftToDepthTable, XnDepthPixel, m_s2dConfig.nDeviceMaxShiftValue+1, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALIGNED_CALLOC(m_ShiftToDepthTables.pDepthToShiftTable, XnUInt16, m_s2dConfig.nDeviceMaxDepthValue+1, XN_DEFAULT_MEM_ALIGN);
m_ShiftToDepthTables.bIsInitialized = TRUE;
// store allocation sizes
m_ShiftToDepthTables.nShiftsCount = m_s2dConfig.nDeviceMaxShiftValue + 1;
m_ShiftToDepthTables.nDepthsCount = m_s2dConfig.nDeviceMaxDepthValue + 1;
// Gera a matriz
XnShiftToDepthUpdate(&m_ShiftToDepthTables, &m_s2dConfig);
return (XN_STATUS_OK);
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XN_DDK_API XnStatus XnShiftToDepthInit(XnShiftToDepthTables* pShiftToDepth, const XnShiftToDepthConfig* pConfig)
{
XN_VALIDATE_INPUT_PTR(pShiftToDepth);
XN_VALIDATE_INPUT_PTR(pConfig);
XN_VALIDATE_ALIGNED_CALLOC(pShiftToDepth->pShiftToDepthTable, XnDepthPixel, pConfig->nDeviceMaxShiftValue+1, XN_DEFAULT_MEM_ALIGN);
XN_VALIDATE_ALIGNED_CALLOC(pShiftToDepth->pDepthToShiftTable, XnUInt16, pConfig->nDeviceMaxDepthValue+1, XN_DEFAULT_MEM_ALIGN);
pShiftToDepth->bIsInitialized = TRUE;
// store allocation sizes
pShiftToDepth->nShiftsCount = pConfig->nDeviceMaxShiftValue + 1;
pShiftToDepth->nDepthsCount = pConfig->nDeviceMaxDepthValue + 1;
return XnShiftToDepthUpdate(pShiftToDepth, pConfig);
}
XN_C_API XnStatus xnOSAcceptSocket(XN_SOCKET_HANDLE ListenSocket, XN_SOCKET_HANDLE* AcceptSocketPtr, XnUInt32 nMillisecondsTimeout)
{
// Local function variables
XnInt32 nRetVal = 0;
struct timeval selectTimeOut;
struct timeval* pTimeout = xnOSMillisecsToTimeVal(nMillisecondsTimeout, &selectTimeOut);
fd_set fdReadHandles;
XN_SOCKET_HANDLE AcceptSocket = NULL;
// Validate the input/output pointers (to make sure none of them is NULL)
XN_VALIDATE_INPUT_PTR(ListenSocket);
XN_VALIDATE_OUTPUT_PTR(AcceptSocketPtr);
// Make sure the actual socket handle isn't NULL
XN_RET_IF_NULL(ListenSocket->Socket, XN_STATUS_OS_INVALID_SOCKET);
// Wait for connection request
XN_PRAGMA_START_DISABLED_WARNING_SECTION(XN_CONDITION_IS_CONST_WARNING_ID);
FD_ZERO(&fdReadHandles);
FD_SET(ListenSocket->Socket, &fdReadHandles);
XN_PRAGMA_STOP_DISABLED_WARNING_SECTION;
nRetVal = select(1 /* ignored */, &fdReadHandles, NULL, NULL, pTimeout);
if (nRetVal == 0)
{
return (XN_STATUS_OS_NETWORK_TIMEOUT);
}
else if (nRetVal == SOCKET_ERROR)
{
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_ACCEPT_FAILED, XN_MASK_OS, "select() returned WinSock error: %d", WSAGetLastError());
}
// Allocate a new socket
XN_VALIDATE_ALIGNED_CALLOC(*AcceptSocketPtr, xnOSSocket, 1, XN_DEFAULT_MEM_ALIGN);
AcceptSocket = *AcceptSocketPtr;
// Accept the socket and make sure it succeeded
AcceptSocket->nSocketAddressLen = sizeof(AcceptSocket->SocketAddress);
AcceptSocket->Socket = accept(ListenSocket->Socket, (sockaddr*)&AcceptSocket->SocketAddress, &AcceptSocket->nSocketAddressLen);
if (AcceptSocket->Socket == INVALID_SOCKET)
{
xnOSCloseSocket(AcceptSocket);
xnOSFreeAligned(*AcceptSocketPtr);
return(XN_STATUS_OS_NETWORK_SOCKET_ACCEPT_FAILED);
}
// All is good...
return (XN_STATUS_OK);
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XnStatus xnFPSInit(XnFPSData* pFPS, XnUInt32 nSamplesCount)
{
XN_VALIDATE_OUTPUT_PTR(pFPS);
// Allocate struct
XN_VALIDATE_CALLOC(*pFPS, XnFPSDataImpl, 1);
XnFPSDataImpl* pData = *pFPS;
// Allocate array
XN_VALIDATE_ALIGNED_CALLOC(pData->anTimes, XnUInt64, nSamplesCount, XN_DEFAULT_MEM_ALIGN);
pData->nArraySize = nSamplesCount;
return XN_STATUS_OK;
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XnStatus xnFPSInit(XnFPSData* pFPS, XnUInt32 nSamplesCount)
{
XN_VALIDATE_OUTPUT_PTR(pFPS);
// make sure xnOS is init
XnStatus nRetVal = xnOSInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_OS_ALREADY_INIT)
return nRetVal;
// Allocate struct
XN_VALIDATE_CALLOC(*pFPS, XnFPSDataImpl, 1);
XnFPSDataImpl* pData = *pFPS;
// Allocate array
XN_VALIDATE_ALIGNED_CALLOC(pData->anTimes, XnUInt64, nSamplesCount, XN_DEFAULT_MEM_ALIGN);
pData->nArraySize = nSamplesCount;
return XN_STATUS_OK;
}
XN_C_API XnStatus xnUSBOpenEndPoint(XN_USB_DEV_HANDLE pDevHandle, XnUInt16 nEndPointID, XnUSBEndPointType nEPType, XnUSBDirectionType nDirType, XN_USB_EP_HANDLE* pEPHandlePtr)
{
// Local variables
XnBool bResult = TRUE;
XnStatus nRetVal = XN_STATUS_OK;
XnInt32 nRetBytes = 0;
XnChar pConfigDescBuf[MAX_CONFIG_DESC_SIZE];
XnChar* pBuf = NULL;
PUSB_CONFIGURATION_DESCRIPTOR pUSBConfigDesc = NULL;
PUSB_INTERFACE_DESCRIPTOR pUSBInterfaceDesc = NULL;
PUSB_ENDPOINT_DESCRIPTOR pUSBEndPointDesc = NULL;
XN_USB_EP_HANDLE pEPHandle = NULL;
XnChar cpPipeID[3];
// Validate xnUSB
XN_VALIDATE_USB_INIT();
XN_VALIDATE_USB_PDEV_HANDLE(pDevHandle);
// Validate the input/output pointers
XN_VALIDATE_OUTPUT_PTR(pEPHandlePtr);
// Allocate a new xnUSB EP handle
XN_VALIDATE_ALIGNED_CALLOC(*pEPHandlePtr, xnUSBEPHandle, 1, XN_DEFAULT_MEM_ALIGN);
pEPHandle = *pEPHandlePtr;
// Read the config descriptor
bResult = DeviceIoControl(pDevHandle->hUSBDevHandle, IOCTL_PSDRV_GET_CONFIG_DESCRIPTOR, pConfigDescBuf, sizeof(pConfigDescBuf), pConfigDescBuf, sizeof(pConfigDescBuf), (PULONG)&nRetBytes, NULL);
if (bResult)
{
XnUInt32 nIFIdx = 0;
UCHAR nEPIdx = 0;
XnUInt32 nUBBEPType = 0;
XnUInt32 nCurrIF = 0;
pBuf = pConfigDescBuf;
pUSBConfigDesc = (PUSB_CONFIGURATION_DESCRIPTOR)pBuf;
pBuf += pUSBConfigDesc->bLength;
// Scan all the interfaces
do {
pUSBInterfaceDesc = (PUSB_INTERFACE_DESCRIPTOR)pBuf;
pBuf += pUSBInterfaceDesc->bLength;
// Scan all the endpoints
for (nEPIdx = 0; nEPIdx < pUSBInterfaceDesc->bNumEndpoints; nEPIdx++)
{
pUSBEndPointDesc = (PUSB_ENDPOINT_DESCRIPTOR)pBuf;
// Is this the EP we're looking for?
if ((pUSBEndPointDesc->bEndpointAddress == nEndPointID) && (pDevHandle->nAltInterface == nCurrIF))
{
// Get the EP type
nUBBEPType = pUSBEndPointDesc->bmAttributes & USB_ENDPOINT_TYPE_MASK;
// Verify that the EP type matches the requested EP
if (nEPType == XN_USB_EP_BULK)
{
if (nUBBEPType != USB_ENDPOINT_TYPE_BULK)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_WRONG_ENDPOINT_TYPE);
}
}
else if (nEPType == XN_USB_EP_INTERRUPT)
{
if (nUBBEPType != USB_ENDPOINT_TYPE_INTERRUPT)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_WRONG_ENDPOINT_TYPE);
}
}
else if (nEPType == XN_USB_EP_ISOCHRONOUS)
{
if (nUBBEPType != USB_ENDPOINT_TYPE_ISOCHRONOUS)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_WRONG_ENDPOINT_TYPE);
}
}
else
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_UNKNOWN_ENDPOINT_TYPE);
}
// Verify that the EP direction matches the requested direction
if (nDirType == XN_USB_DIRECTION_IN)
{
if (USB_ENDPOINT_DIRECTION_IN(pUSBEndPointDesc->bEndpointAddress) == FALSE)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_WRONG_ENDPOINT_DIRECTION);
}
}
else if (nDirType == XN_USB_DIRECTION_OUT)
{
if (USB_ENDPOINT_DIRECTION_OUT(pUSBEndPointDesc->bEndpointAddress) == FALSE)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_WRONG_ENDPOINT_DIRECTION);
}
}
else
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_UNKNOWN_ENDPOINT_DIRECTION);
}
// Construct the pipe file name
pEPHandle->cpPipeName[0] = 0;
cpPipeID[0] = '0';
cpPipeID[1] = '0' + nEPIdx;
cpPipeID[2] = 0;
StringCchCopy(pEPHandle->cpPipeName, MAX_DEVICE_STR_LENGTH, pDevHandle->cpDeviceName);
StringCchCat(pEPHandle->cpPipeName, MAX_DEVICE_STR_LENGTH, PSDRV_PIPE_PREFIX);
StringCchCat(pEPHandle->cpPipeName, MAX_DEVICE_STR_LENGTH, cpPipeID);
// Open the regular pipe handle
pEPHandle->hEPHandle = CreateFile(pEPHandle->cpPipeName, GENERIC_WRITE | GENERIC_READ, FILE_SHARE_WRITE | FILE_SHARE_READ, NULL, OPEN_EXISTING, NULL, NULL);
if (pEPHandle->hEPHandle == INVALID_HANDLE_VALUE)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_OPEN_ENDPOINT_FAILED);
}
// Init the overlapped I/O structs
nRetVal = xnUSBInitOvlp(pEPHandle);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (XN_STATUS_USB_OPEN_ENDPOINT_FAILED);
}
// Init the ThreadData variables
xnOSMemSet(&pEPHandle->ThreadData, 0, sizeof(xnUSBReadThreadData));
pEPHandle->ThreadData.bInUse = FALSE;
// Init the default endpoint properties
pEPHandle->nTimeOut = XN_USB_DEFAULT_EP_TIMEOUT;
pEPHandle->nEPType = nEPType;
pEPHandle->nEPDir = nDirType;
pEPHandle->nEndPointID = nEndPointID;
// Set the default endpoint timeout
nRetVal = xnUSBSetPipeProperty(pEPHandle, PSUSBDRV_PIPE_PROPERTY_TIMEOUT, XN_USB_DEFAULT_EP_TIMEOUT);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pEPHandle);
return (nRetVal);
}
if (nUBBEPType == USB_ENDPOINT_TYPE_ISOCHRONOUS)
{
// bits 11 and 12 mark the number of additional transactions, bits 0-10 mark the size
XnUInt32 nAdditionalTransactions = pUSBEndPointDesc->wMaxPacketSize >> 11;
XnUInt32 nPacketSize = pUSBEndPointDesc->wMaxPacketSize & 0x7FF;
pEPHandle->nMaxPacketSize = (nAdditionalTransactions + 1) * (nPacketSize);
}
else
{
pEPHandle->nMaxPacketSize = pUSBEndPointDesc->wMaxPacketSize;
}
// Mark the endpoint as valid
pEPHandle->bValid = TRUE;
// The end... (Happy)
return (XN_STATUS_OK);
}
pBuf += pUSBEndPointDesc->bLength;
}
XnStatus xnUSBOpenDeviceImpl(const XnChar* strDevicePath, XN_USB_DEV_HANDLE* pDevHandlePtr)
{
// Local variables
XnStatus nRetVal = XN_STATUS_OK;
LPCGUID pInterfaceGuid = NULL;
XN_USB_DEV_HANDLE pDevHandle = NULL;
ULONG nNumberDevices = 0;
HDEVINFO hDevInfo = NULL;
BOOLEAN bDone = FALSE;
SP_DEVICE_INTERFACE_DATA devInterfaceData;
PUSB_DEVICE_DESCRIPTOR pUsbDeviceInst = NULL;
PUSB_DEVICE_DESCRIPTOR* ppUsbDevices = &pUsbDeviceInst;
PUSB_DEVICE_DESCRIPTOR pTempDevDesc = NULL;
ULONG nIdx = 0;
HANDLE hSelectedDevice = INVALID_HANDLE_VALUE;
PSUSBDRV_DRIVER_VERSION UsbDriverVersion;
PSUSBDRV_INTERFACE_PROPERTY pInterfaceProp;
// Validate xnUSB
XN_VALIDATE_USB_INIT();
// Validate the input/output pointers
XN_VALIDATE_OUTPUT_PTR(pDevHandlePtr);
// Allocate a new xnUSB Device handle
XN_VALIDATE_ALIGNED_CALLOC(*pDevHandlePtr, XnUSBDeviceHandle, 1, XN_DEFAULT_MEM_ALIGN);
pDevHandle = *pDevHandlePtr;
// Get Device Path
pInterfaceGuid = &GUID_CLASS_PSDRV_USB;
// See if the driver is installed
hDevInfo = SetupDiGetClassDevs (pInterfaceGuid, NULL, NULL, (DIGCF_PRESENT | DIGCF_DEVICEINTERFACE));
if (hDevInfo == INVALID_HANDLE_VALUE)
{
// No devices are present...
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (XN_STATUS_USB_DRIVER_NOT_FOUND);
}
// Guess how much devices we're going to have. If it's too little, we'll realloc it soon.
nNumberDevices = 4;
// Scan the hardware for any devices that are attached to our driver. Stop only after we have successfully opened a device.
devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
nIdx = 0;
bDone = FALSE;
while (!bDone)
{
// Allocate enough memory for all the devices that are attached to the driver
nNumberDevices *= 2;
if (*ppUsbDevices)
{
pTempDevDesc = (USB_DEVICE_DESCRIPTOR*)realloc(*ppUsbDevices, (nNumberDevices * sizeof (USB_DEVICE_DESCRIPTOR)));
if(pTempDevDesc)
{
// We have enough memory!
*ppUsbDevices = pTempDevDesc;
pTempDevDesc = NULL;
}
else
{
// Out of memory... realloc failed...
free(*ppUsbDevices);
*ppUsbDevices = NULL;
}
}
else
{
*ppUsbDevices = (USB_DEVICE_DESCRIPTOR*)calloc (nNumberDevices, sizeof (USB_DEVICE_DESCRIPTOR));
}
// Make sure we have found some devices
if (NULL == *ppUsbDevices)
{
SetupDiDestroyDeviceInfoList(hDevInfo);
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (XN_STATUS_USB_DEVICE_GETINFO_FAILED);
}
pUsbDeviceInst = *ppUsbDevices + nIdx;
// Scan all the devices that are attached to the driver. Stop when one of them open successfully.
for (; nIdx < nNumberDevices; nIdx++)
{
// Get information about the device
if (SetupDiEnumDeviceInterfaces (hDevInfo, 0, pInterfaceGuid, nIdx, &devInterfaceData))
{
// Try to open this device
hSelectedDevice = xnUSBOpenOneDevice(hDevInfo, &devInterfaceData, pDevHandle->cpDeviceName, strDevicePath);
if (hSelectedDevice != INVALID_HANDLE_VALUE)
{
// Success! We have a valid device handle.
bDone = TRUE;
break;
}
}
else
{
// Did we reach the end?
if (ERROR_NO_MORE_ITEMS == GetLastError())
{
// Yup... and we didn't find any devices...
bDone = TRUE;
break;
}
}
}
}
// Save the number of devices
nNumberDevices = nIdx;
// Cleanup memory
SetupDiDestroyDeviceInfoList (hDevInfo);
free (*ppUsbDevices);
// Do we have a valid device?
if (hSelectedDevice == INVALID_HANDLE_VALUE)
{
// Nope...
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (XN_STATUS_USB_DEVICE_NOT_FOUND);
}
// Save the device handle
pDevHandle->hUSBDevHandle = hSelectedDevice;
// Get the device speed
nRetVal = xnUSBGetDeviceSpeedInternal(pDevHandle, &pDevHandle->nDevSpeed);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
// Get the driver version
nRetVal = xnUSBGetDriverVersion(pDevHandle, &UsbDriverVersion);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
xnLogVerbose(XN_MASK_USB, "USB Driver Version is: %d.%d.%d.%d", UsbDriverVersion.nMajor, UsbDriverVersion.nMinor, UsbDriverVersion.nMaintenance, UsbDriverVersion.nBuild);
// Mark the device handle as valid
pDevHandle->bValid = TRUE;
// Read the current alt-if settings
nRetVal = xnUSBGetInterface(pDevHandle, &pInterfaceProp.nIF, &pInterfaceProp.nAltIF);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
pDevHandle->nAltInterface = pInterfaceProp.nAltIF;
xnLogVerbose(XN_MASK_USB, "USB Driver Current Alt Setting is: %d", pDevHandle->nAltInterface);
// All is good...
return (XN_STATUS_OK);
}
XN_C_API XnStatus xnOSCreateSocket(const XnOSSocketType SocketType, const XnChar* cpIPAddress, const XnUInt16 nPort, XN_SOCKET_HANDLE* SocketPtr)
{
// Local function variables
hostent* HostEnt = NULL;
XN_SOCKET_HANDLE Socket = NULL;
// Validate the input/output pointers (to make sure none of them is NULL)
XN_VALIDATE_INPUT_PTR(cpIPAddress);
XN_VALIDATE_OUTPUT_PTR(SocketPtr);
XN_VALIDATE_ALIGNED_CALLOC(*SocketPtr, xnOSSocket, 1, XN_DEFAULT_MEM_ALIGN);
Socket = *SocketPtr;
if (SocketType == XN_OS_UDP_SOCKET)
{
// Create a UDP socket
Socket->Socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
}
else if (SocketType == XN_OS_TCP_SOCKET)
{
// Create a TCP socket
Socket->Socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
}
else
{
// Unknown socket type...
XN_ALIGNED_FREE_AND_NULL(Socket);
return (XN_STATUS_OS_NETWORK_INVALID_SOCKET_TYPE);
}
if (Socket->Socket == INVALID_SOCKET)
{
XN_ALIGNED_FREE_AND_NULL(Socket);
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_CREATION_FAILED, XN_MASK_OS, "socket() returned WinSock error: %d", WSAGetLastError());
}
// Set the socket server address
Socket->SocketAddress.sin_family = AF_INET;
if (isalpha(cpIPAddress[0]))
{
HostEnt = gethostbyname(cpIPAddress);
if (HostEnt == NULL)
{
XN_ALIGNED_FREE_AND_NULL(Socket);
return (XN_STATUS_OS_NETWORK_BAD_HOST_NAME);
}
xnOSMemCopy(&Socket->SocketAddress.sin_addr, HostEnt->h_addr, HostEnt->h_length);
}
else
{
Socket->SocketAddress.sin_addr.s_addr = inet_addr(cpIPAddress);
}
Socket->SocketAddress.sin_port = htons(nPort);
// Update socket address size
Socket->nSocketAddressLen = sizeof(Socket->SocketAddress);
// Remember the socket type
Socket->nSocketType = SocketType;
// All is good...
return (XN_STATUS_OK);
}
XnStatus XnFileDevice::BCReadInitialState(XnPropertySet* pSet)
{
// Local function variables
XnStatus nRetVal = XN_STATUS_OK;
XnDeviceFileHeader DeviceFileHeader;
XN_STREAM_FLAGS_TYPE nStreamFlags = 0;
m_pBCData->nFramePos = 1;
xnOSFreeAligned(m_pBCData->pPackedStreamBuffer);
m_pBCData->pPackedStreamBuffer = NULL;
m_pBCData->nPackedStreamBufferSize = 0;
// read StreamProperties
if (m_nFileVersion == 3)
{
// Current Version
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.nMajorVersion, sizeof(XnUInt16));
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.nMinorVersion, sizeof(XnUInt16));
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.StreamProperties, sizeof(XnStreamPropertiesV3));
XN_IS_STATUS_OK(nRetVal);
DeviceFileHeader.nMajorVersion = XN_PREPARE_VAR16_IN_BUFFER(DeviceFileHeader.nMajorVersion);
DeviceFileHeader.nMinorVersion = XN_PREPARE_VAR16_IN_BUFFER(DeviceFileHeader.nMinorVersion);
nRetVal = XnIOAdjustStreamPropertiesV3(&DeviceFileHeader.StreamProperties, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else if (m_nFileVersion == 2)
{
// Version 2
DeviceFileHeader.nMajorVersion = 0;
DeviceFileHeader.nMinorVersion = 0;
XnStreamPropertiesV2 StreamPropertiesV2;
nRetVal = m_pInputStream->ReadData((XnUChar*)&StreamPropertiesV2, sizeof(XnStreamPropertiesV2));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustStreamPropertiesV2(&StreamPropertiesV2, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else if (m_nFileVersion == 1)
{
// Version 1
DeviceFileHeader.nMajorVersion = 0;
DeviceFileHeader.nMinorVersion = 0;
XnStreamPropertiesV1 StreamPropertiesV1;
nRetVal = m_pInputStream->ReadData((XnUChar*)&StreamPropertiesV1, sizeof(XnStreamPropertiesV1));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustStreamPropertiesV1(&StreamPropertiesV1, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else
{
// Bad Magic
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
// read packed stream properties
switch (m_nFileVersion)
{
case 3:
{
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.PackedStreamProperties, sizeof(XnPackedStreamProperties));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV3(&DeviceFileHeader.PackedStreamProperties, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 2:
{
XnPackedStreamPropertiesV2 PackedStreamPropertiesV2;
nRetVal = m_pInputStream->ReadData((XnUChar*)&PackedStreamPropertiesV2, sizeof(XnPackedStreamPropertiesV2));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV2(&PackedStreamPropertiesV2, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 1:
{
XnPackedStreamPropertiesV1 PackedStreamPropertiesV1;
nRetVal = m_pInputStream->ReadData((XnUChar*)&PackedStreamPropertiesV1, sizeof(XnPackedStreamPropertiesV1));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV1(&PackedStreamPropertiesV1, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
default:
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
// Save the stream properties into the private data (but keep the original stream flags)
nStreamFlags = m_pBCData->StreamProperties.nStreamFlags;
xnOSMemCopy(&m_pBCData->StreamProperties, &DeviceFileHeader.StreamProperties, sizeof(XnStreamProperties));
m_pBCData->StreamProperties.nStreamFlags = nStreamFlags;
if (m_pBCData->StreamProperties.Shift2DepthData.bShift2DepthData)
{
m_pBCData->StreamProperties.Shift2DepthData.nMaxDepthValue = 10000;
m_pBCData->StreamProperties.nDepthMaxValue = 10000;
}
// Save the packed stream properties into the private data
xnOSMemCopy(&m_pBCData->PackedStreamProperties, &DeviceFileHeader.PackedStreamProperties, sizeof(XnPackedStreamProperties));
XnUInt32 nBufferSize = BCCalculatePackedBufferSize();
if (nBufferSize != m_pBCData->nPackedStreamBufferSize)
{
xnOSFreeAligned(m_pBCData->pPackedStreamBuffer);
XN_VALIDATE_ALIGNED_CALLOC(m_pBCData->pPackedStreamBuffer, XnUChar, nBufferSize, XN_DEFAULT_MEM_ALIGN);
m_pBCData->nPackedStreamBufferSize = nBufferSize;
}
nRetVal = ConvertStreamPropertiesToPropertySet(&m_pBCData->StreamProperties, &m_pBCData->PackedStreamProperties, pSet);
XN_IS_STATUS_OK(nRetVal);
// All is good...
return (XN_STATUS_OK);
}
XnStatus xnUSBOpenDeviceImpl(const XnChar* strDevicePath, XN_USB_DEV_HANDLE* pDevHandlePtr)
{
// Local variables
XnStatus nRetVal = XN_STATUS_OK;
LPCGUID pInterfaceGuid = NULL;
XN_USB_DEV_HANDLE pDevHandle = NULL;
HDEVINFO hDevInfo = NULL;
SP_DEVICE_INTERFACE_DATA devInterfaceData;
HANDLE hSelectedDevice = INVALID_HANDLE_VALUE;
PSUSBDRV_DRIVER_VERSION UsbDriverVersion;
PSUSBDRV_INTERFACE_PROPERTY pInterfaceProp;
// Validate xnUSB
XN_VALIDATE_USB_INIT();
// Validate the input/output pointers
XN_VALIDATE_OUTPUT_PTR(pDevHandlePtr);
// Allocate a new xnUSB Device handle
XN_VALIDATE_ALIGNED_CALLOC(*pDevHandlePtr, XnUSBDeviceHandle, 1, XN_DEFAULT_MEM_ALIGN);
pDevHandle = *pDevHandlePtr;
// Get Device Path
pInterfaceGuid = &GUID_CLASS_PSDRV_USB;
// See if the driver is installed
hDevInfo = SetupDiGetClassDevs (pInterfaceGuid, NULL, NULL, (DIGCF_PRESENT | DIGCF_DEVICEINTERFACE));
if (hDevInfo == INVALID_HANDLE_VALUE)
{
// No devices are present...
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (XN_STATUS_USB_DRIVER_NOT_FOUND);
}
// Scan the hardware for any devices that are attached to our driver. Stop only after we have successfully opened a device.
devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
bool bDone = FALSE;
ULONG nIdx = 0;
// Scan all the devices that are attached to the driver. Stop when one of them open successfully.
while (!bDone)
{
// Get information about the device
if (SetupDiEnumDeviceInterfaces (hDevInfo, 0, pInterfaceGuid, nIdx, &devInterfaceData))
{
// Try to open this device
hSelectedDevice = xnUSBOpenOneDevice(hDevInfo, &devInterfaceData, pDevHandle->cpDeviceName, strDevicePath);
if (hSelectedDevice != INVALID_HANDLE_VALUE)
{
// Success! We have a valid device handle.
bDone = TRUE;
break;
}
}
else
{
// Did we reach the end?
if (ERROR_NO_MORE_ITEMS == GetLastError())
{
// Yup... and we didn't find any devices...
bDone = TRUE;
break;
}
}
++nIdx;
}
// Cleanup memory
SetupDiDestroyDeviceInfoList (hDevInfo);
// Do we have a valid device?
if (hSelectedDevice == INVALID_HANDLE_VALUE)
{
// Nope...
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (XN_STATUS_USB_DEVICE_NOT_FOUND);
}
// Save the device handle
pDevHandle->hUSBDevHandle = hSelectedDevice;
// Get the device speed
nRetVal = xnUSBGetDeviceSpeedInternal(pDevHandle, &pDevHandle->nDevSpeed);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
// Get the driver version
nRetVal = xnUSBGetDriverVersion(pDevHandle, &UsbDriverVersion);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
xnLogVerbose(XN_MASK_USB, "USB Driver Version is: %d.%d.%d.%d", UsbDriverVersion.nMajor, UsbDriverVersion.nMinor, UsbDriverVersion.nMaintenance, UsbDriverVersion.nBuild);
// Mark the device handle as valid
pDevHandle->bValid = TRUE;
// Read the current alt-if settings
nRetVal = xnUSBGetInterface(pDevHandle, &pInterfaceProp.nIF, &pInterfaceProp.nAltIF);
if (nRetVal != XN_STATUS_OK)
{
XN_ALIGNED_FREE_AND_NULL(pDevHandle);
return (nRetVal);
}
pDevHandle->nAltInterface = pInterfaceProp.nAltIF;
xnLogVerbose(XN_MASK_USB, "USB Driver Current Alt Setting is: %d", pDevHandle->nAltInterface);
// All is good...
return (XN_STATUS_OK);
}
