/*! \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; }