Main MRPT website > C++ reference for MRPT 1.9.9
enumerateusbdevices.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #include "xcommunicationconfig.h"
10 #include "enumerateusbdevices.h"
11 
12 #ifdef _WIN32
13 #include <windows.h>
14 #include <string.h>
15 #include <setupapi.h>
16 #include <devguid.h>
17 #include <regstr.h>
18 #else
19 #include <stdlib.h>
20 #include <string.h>
21 #include <dirent.h>
22 #include "xslibusb.h"
23 #endif
24 
25 #define XSENS_VENDOR_ID 0x2639
26 #define ATMEL_VENDOR_ID 0x03eb // needed for old MTw
27 #define ATMEL_BORROWED_PRODUCT_ID 0x2307 // needed for old MTw
28 
29 /*! \brief Enumerate Xsens USB devices
30 
31  If the OS already has drivers running for a device, the device should
32  already have been
33  found by xsEnumerateSerialPorts().
34 
35  \param[in,out] ports The list of serial ports to append to
36 */
38 {
39  XsPortInfo current;
40 #ifdef USE_WINUSB
41  BOOL bResult = FALSE;
42  ULONG length;
43  ULONG requiredLength = 0;
44 
45  // {FD51225C-700A-47e5-9999-B2D9031B88ED}
46  GUID guid = {0xfd51225c,
47  0x700a,
48  0x47e5,
49  {0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed}};
50 
51  HDEVINFO deviceInfo;
52  SP_DEVICE_INTERFACE_DATA interfaceData;
53  PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = nullptr;
54 
55  deviceInfo = SetupDiGetClassDevs(
56  &guid, nullptr, nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);
57 
58  // Initialize variables.
59  interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA);
60  int port = 0;
61  for (DWORD dwIndex = 0; port == 0; ++dwIndex)
62  {
63  BOOL bRet = SetupDiEnumDeviceInterfaces(
64  deviceInfo, nullptr, &guid, dwIndex, &interfaceData);
65  if (!bRet)
66  {
67  if (GetLastError() == ERROR_NO_MORE_ITEMS) break;
68  }
69  else
70  {
71  if (!SetupDiGetDeviceInterfaceDetail(
72  deviceInfo, &interfaceData, nullptr, 0, &requiredLength,
73  nullptr))
74  {
75  if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
76  {
77  SetupDiDestroyDeviceInfoList(deviceInfo);
78  return false;
79  }
80  }
81  detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(
82  LMEM_FIXED, requiredLength);
83  if (nullptr == detailData)
84  {
85  SetupDiDestroyDeviceInfoList(deviceInfo);
86  return false;
87  }
88 
89  detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
90  length = requiredLength;
91  SP_DEVINFO_DATA DevInfoData;
92  DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
93  bResult = SetupDiGetDeviceInterfaceDetailA(
94  deviceInfo, &interfaceData, detailData, length, &requiredLength,
95  &DevInfoData);
96 
97  if (!bResult)
98  {
99  LocalFree(detailData);
100  SetupDiDestroyDeviceInfoList(deviceInfo);
101  return false;
102  }
103 
104  unsigned char serialNumber[256];
105  char *ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#');
106  if (!ptrStart) continue;
107  ptrStart = strchr(ptrStart + 1, '#');
108  if (!ptrStart) continue;
109  ptrEnd = strchr(ptrStart + 1, '#');
110  if (!ptrEnd) continue;
111 
112  strncpy((char*)serialNumber, ptrStart + 1, ptrEnd - ptrStart - 1);
113  serialNumber[ptrEnd - ptrStart - 1] = '\0';
114 
115  current.setPortName(detailData->DevicePath);
116 
117  int id = 0;
118  sscanf((const char*)serialNumber, "%X", &id);
119  current.setDeviceId((uint32_t)id);
120 
121  ports.push_back(current);
122  }
123  }
124 
125  SetupDiDestroyDeviceInfoList(deviceInfo);
126  return true;
127 #else
128  XsLibUsb libUsb;
129  libusb_context* context;
130  int result = libUsb.init(&context);
131  if (result != LIBUSB_SUCCESS) return false;
132 
133  libusb_device** deviceList;
134  ssize_t deviceCount = libUsb.get_device_list(context, &deviceList);
135  for (ssize_t i = 0; i < deviceCount; i++)
136  {
137  libusb_device* device = deviceList[i];
138  libusb_device_descriptor desc;
139  result = libUsb.get_device_descriptor(device, &desc);
140  if (result != LIBUSB_SUCCESS) continue;
141 
142  if (desc.idVendor != XSENS_VENDOR_ID &&
143  desc.idVendor != ATMEL_VENDOR_ID)
144  continue;
145 
146  libusb_device_handle* handle;
147  result = libUsb.open(device, &handle);
148  if (result != LIBUSB_SUCCESS)
149  {
150  libUsb.unref_device(device);
151  continue;
152  }
153 
154  unsigned char serialNumber[256];
155  result = libUsb.get_string_descriptor_ascii(
156  handle, desc.iSerialNumber, serialNumber, 256);
157 
158  if (desc.idVendor == ATMEL_VENDOR_ID &&
159  desc.idProduct == ATMEL_BORROWED_PRODUCT_ID)
160  {
161  unsigned char productName[256];
162  result = libUsb.get_string_descriptor_ascii(
163  handle, desc.iProduct, productName, 256);
164 
165  if (strcmp("Xsens COM port", (const char*)productName) != 0)
166  {
167  libUsb.close(handle);
168  continue;
169  }
170  }
171 
172  libusb_config_descriptor* configDesc;
173  result = libUsb.get_active_config_descriptor(device, &configDesc);
174  if (result != LIBUSB_SUCCESS) continue;
175 
176  bool kernelActive = false;
177  for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces;
178  ++ifCount)
179  {
180  int res = libUsb.kernel_driver_active(handle, ifCount);
181  kernelActive |= (res == 1);
182  }
183 
184  libUsb.free_config_descriptor(configDesc);
185 
186  if (!kernelActive)
187  {
188  char name[256];
189  sprintf(
190  name, "USB%03u:%03u", libUsb.get_bus_number(device),
191  libUsb.get_device_address(device));
192  current.setPortName(name);
193 
194  int id = 0;
195  sscanf((const char*)serialNumber, "%d", &id);
196  current.setDeviceId((uint32_t)id);
197  ports.push_back(current);
198  }
199  else
200  {
201  JLDEBUG(
202  gJournal, "Kernel driver active on USB"
203  << libUsb.get_bus_number(device) << ":"
204  << libUsb.get_device_address(device) << " device "
205  << serialNumber);
206  }
207  libUsb.close(handle);
208  }
209  libUsb.free_device_list(deviceList, 1);
210  libUsb.exit(context);
211  return true;
212 #endif
213 }
#define XSENS_VENDOR_ID
libUSB_close close
Definition: xslibusb.h:61
libUSB_unref_device unref_device
Definition: xslibusb.h:66
libUSB_open open
Definition: xslibusb.h:60
libUSB_init init
Definition: xslibusb.h:58
libUSB_free_config_descriptor free_config_descriptor
Definition: xslibusb.h:70
#define ATMEL_BORROWED_PRODUCT_ID
int BOOL
Definition: xstypedefs.h:76
libUSB_get_string_descriptor_ascii get_string_descriptor_ascii
Definition: xslibusb.h:77
unsigned char uint8_t
Definition: rptypes.h:41
libUSB_free_device_list free_device_list
Definition: xslibusb.h:76
bool xsEnumerateUsbDevices(XsPortInfoList &ports)
Enumerate Xsens USB devices.
#define FALSE
Definition: jmorecfg.h:216
libUSB_get_device_list get_device_list
Definition: xslibusb.h:75
libUSB_get_device_descriptor get_device_descriptor
Definition: xslibusb.h:74
libUSB_kernel_driver_active kernel_driver_active
Definition: xslibusb.h:62
libUSB_exit exit
Definition: xslibusb.h:59
#define JLDEBUG(...)
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
GLuint const GLchar * name
Definition: glext.h:4054
libUSB_get_device_address get_device_address
Definition: xslibusb.h:73
GLuint res
Definition: glext.h:7268
unsigned __int32 uint32_t
Definition: rptypes.h:47
libUSB_get_bus_number get_bus_number
Definition: xslibusb.h:71
libUSB_get_active_config_descriptor get_active_config_descriptor
Definition: xslibusb.h:69
#define XsPortInfoList
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:188
#define ATMEL_VENDOR_ID



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019