XnStatus OpenNIMotorController::GetInformation( int& rAngle, MOTOR_STATUS& rMotorStatus, XnVector3D& rVec ) const { XnUChar aData[10]; XnUInt32 uSize; XnStatus res = xnUSBReceiveControl( m_xDevice, XN_USB_CONTROL_TYPE_VENDOR, 0x32, 0x00, 0x00, aData, 10, &uSize, 0 ); if( res == XN_STATUS_OK ) { rAngle = aData[8]; if( rAngle > 128 ) rAngle = -0.5 * ( 255 - rAngle ); else rAngle /= 2; if( aData[9] == 0x00 ) rMotorStatus = MOTOR_STOPPED; else if( aData[9] == 0x01 ) rMotorStatus = MOTOR_LIMIT; else if( aData[9] == 0x04 ) rMotorStatus = MOTOR_MOVING; else rMotorStatus = MOTOR_UNKNOWN; rVec.X = (float)( ((XnUInt16)aData[2] << 8) | aData[3] ); rVec.Y = (float)( ((XnUInt16)aData[4] << 8) | aData[5] ); rVec.Z = (float)( ((XnUInt16)aData[6] << 8) | aData[7] ); } return res; }
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; }