/*! \brief Enumerate Xsens USB devices
If the OS already has drivers running for a device, the device should already have been
found by xsEnumerateSerialPorts().
\param[in,out] ports The list of serial ports to append to
*/
bool xsEnumerateUsbDevices(XsPortInfoList& ports)
{
XsPortInfo current;
#ifdef USE_WINUSB
BOOL bResult = FALSE;
ULONG length;
ULONG requiredLength=0;
// {FD51225C-700A-47e5-9999-B2D9031B88ED}
GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } };
HDEVINFO deviceInfo;
SP_DEVICE_INTERFACE_DATA interfaceData;
PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = NULL;
deviceInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);
// Initialize variables.
interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA);
int port = 0;
for (DWORD dwIndex = 0; port == 0; ++dwIndex)
{
BOOL bRet = SetupDiEnumDeviceInterfaces( deviceInfo, NULL, &guid, dwIndex, &interfaceData);
if (!bRet)
{
if (GetLastError() == ERROR_NO_MORE_ITEMS)
break;
}
else
{
if (!SetupDiGetDeviceInterfaceDetail(deviceInfo, &interfaceData, NULL, 0, &requiredLength, NULL))
{
if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
{
SetupDiDestroyDeviceInfoList(deviceInfo);
return false;
}
}
detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(LMEM_FIXED, requiredLength);
if (NULL == detailData)
{
SetupDiDestroyDeviceInfoList(deviceInfo);
return false;
}
detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
length = requiredLength;
SP_DEVINFO_DATA DevInfoData;
DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
bResult = SetupDiGetDeviceInterfaceDetailA(deviceInfo, &interfaceData, detailData, length, &requiredLength, &DevInfoData);
if (!bResult)
{
LocalFree(detailData);
SetupDiDestroyDeviceInfoList(deviceInfo);
return false;
}
unsigned char serialNumber[256];
char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#');
if (!ptrStart)
continue;
ptrStart = strchr(ptrStart+1, '#');
if (!ptrStart)
continue;
ptrEnd = strchr(ptrStart+1, '#');
if (!ptrEnd)
continue;
strncpy((char*)serialNumber, ptrStart+1, ptrEnd-ptrStart-1);
serialNumber[ptrEnd-ptrStart-1] = '\0';
current.setPortName(detailData->DevicePath);
int id = 0;
sscanf((const char *)serialNumber, "%X", &id);
current.setDeviceId((uint32_t) id);
ports.push_back(current);
}
}
SetupDiDestroyDeviceInfoList(deviceInfo);
return true;
#elif defined(HAVE_LIBUSB)
XsLibUsb libUsb;
libusb_context *context;
int result = libUsb.init(&context);
if (result != LIBUSB_SUCCESS)
return true;
libusb_device **deviceList;
ssize_t deviceCount = libUsb.get_device_list(context, &deviceList);
for (ssize_t i = 0; i < deviceCount; i++)
{
libusb_device *device = deviceList[i];
libusb_device_descriptor desc;
result = libUsb.get_device_descriptor(device, &desc);
if (result != LIBUSB_SUCCESS)
continue;
if (desc.idVendor != XSENS_VENDOR_ID && desc.idVendor != ATMEL_VENDOR_ID)
continue;
libusb_device_handle *handle;
result = libUsb.open(device, &handle);
if (result != LIBUSB_SUCCESS)
continue;
unsigned char serialNumber[256];
result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256);
if (desc.idVendor == ATMEL_VENDOR_ID && desc.idProduct == ATMEL_BORROWED_PRODUCT_ID)
{
unsigned char productName[256];
result = libUsb.get_string_descriptor_ascii(handle, desc.iProduct, productName, 256);
if (strcmp("Xsens COM port", (const char *)productName) != 0)
{
libUsb.close(handle);
continue;
}
}
libusb_config_descriptor *configDesc;
result = libUsb.get_active_config_descriptor(device, &configDesc);
if (result != LIBUSB_SUCCESS)
{
libUsb.close(handle);
continue;
}
bool kernelActive = false;
for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) {
int res = libUsb.kernel_driver_active(handle, ifCount);
kernelActive |= (res == 1);
}
libUsb.free_config_descriptor(configDesc);
if (!kernelActive)
{
char name[256];
sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device),
libUsb.get_device_address(device));
current.setPortName(name);
int id = 0;
sscanf((const char *)serialNumber, "%X", &id);
current.setDeviceId((uint32_t) id);
ports.push_back(current);
}
else
{
JLDEBUG(gJournal, "Kernel driver active on USB" <<
libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) <<
" device " << serialNumber);
}
libUsb.close(handle);
}
libUsb.free_device_list(deviceList, 1);
libUsb.exit(context);
return true;
#else
(void)ports;
return false;
#endif
}
/*-------------------------------------------------------------
initialize
-------------------------------------------------------------*/
void CIMUXSens_MT4::initialize()
{
#if MRPT_HAS_xSENS_MT4
m_state = ssInitializing;
try
{
// Try to open a specified device, or scan the bus?
XsPortInfoArray portInfoArray;
if (m_portname.empty())
{
if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
xsEnumerateUsbDevices(portInfoArray);
if (portInfoArray.empty())
THROW_EXCEPTION("CIMUXSens_MT4: No 'portname' was specified and no XSens device was found after scanning the system!")
if (m_verbose) cout << "[CIMUXSens_MT4] Found " << portInfoArray.size() <<" devices. Opening the first one.\n";
}
else
{
XsPortInfo portInfo(m_portname, XsBaud::numericToRate(m_port_bauds));
if (m_verbose) cout << "[CIMUXSens_MT4] Using user-supplied portname '"<<m_portname<<"' at "<<m_port_bauds<<" baudrate.\n";
portInfoArray.push_back(portInfo);
}
// Use the first detected device
XsPortInfo mtPort = portInfoArray.at(0);
// Open the port with the detected device
cout << "[CIMUXSens_MT4] Opening port " << mtPort.portName().toStdString() << std::endl;
if (!my_xsens_device.openPort(mtPort))
throw std::runtime_error("Could not open port. Aborting.");
// Put the device in configuration mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into configuration mode...\n";
if (!my_xsens_device.gotoConfig()) // Put the device into configuration mode before configuring the device
throw std::runtime_error("Could not put device into configuration mode. Aborting.");
// Request the device Id to check the device type
mtPort.setDeviceId(my_xsens_device.getDeviceId());
my_xsens_devid = mtPort.deviceId();
// Check if we have an MTi / MTx / MTmk4 device
if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
{
throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
}
cout << "[CIMUXSens_MT4] Found a device with id: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << std::endl;
// Print information about detected MTi / MTx / MTmk4 device
if (m_verbose) cout << "[CIMUXSens_MT4] Device: " << my_xsens_device.getProductCode().toStdString() << " opened." << std::endl;
// Configure the device. Note the differences between MTix and MTmk4
if (m_verbose) cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
if (mtPort.deviceId().isMtix())
{
XsOutputMode outputMode = XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; // output orientation data as quaternion
// set the device configuration
if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
throw std::runtime_error("Could not configure MT device. Aborting.");
}
else if (mtPort.deviceId().isMtMk4())
{
XsOutputConfigurationArray configArray;
configArray.push_back( XsOutputConfiguration(XDI_SampleTime64,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeFine,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_SampleTimeCoarse,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Quaternion,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Temperature,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_Acceleration,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_RateOfTurn,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_MagneticField,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_VelocityXYZ,m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_StatusByte, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_LatLon, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_UtcTime, m_sampleFreq) );
configArray.push_back( XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq) );
if (!my_xsens_device.setOutputConfiguration(configArray))
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
else
{
throw std::runtime_error("Unknown device while configuring. Aborting.");
}
// Put the device in measurement mode
if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
if (!my_xsens_device.gotoMeasurement())
throw std::runtime_error("Could not put device into measurement mode. Aborting.");
m_state = ssWorking;
}
