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;
}
