Main MRPT website > C++ reference for MRPT 1.5.7
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 already have been
32  found by xsEnumerateSerialPorts().
33 
34  \param[in,out] ports The list of serial ports to append to
35 */
37 {
38  XsPortInfo current;
39 #ifdef USE_WINUSB
40  BOOL bResult = FALSE;
41  ULONG length;
42  ULONG requiredLength=0;
43 
44  // {FD51225C-700A-47e5-9999-B2D9031B88ED}
45  GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } };
46 
47  HDEVINFO deviceInfo;
48  SP_DEVICE_INTERFACE_DATA interfaceData;
49  PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = NULL;
50 
51  deviceInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);
52 
53  // Initialize variables.
54  interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA);
55  int port = 0;
56  for (DWORD dwIndex = 0; port == 0; ++dwIndex)
57  {
58  BOOL bRet = SetupDiEnumDeviceInterfaces( deviceInfo, NULL, &guid, dwIndex, &interfaceData);
59  if (!bRet)
60  {
61  if (GetLastError() == ERROR_NO_MORE_ITEMS)
62  break;
63  }
64  else
65  {
66  if (!SetupDiGetDeviceInterfaceDetail(deviceInfo, &interfaceData, NULL, 0, &requiredLength, NULL))
67  {
68  if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
69  {
70  SetupDiDestroyDeviceInfoList(deviceInfo);
71  return false;
72  }
73  }
74  detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(LMEM_FIXED, requiredLength);
75  if (NULL == detailData)
76  {
77  SetupDiDestroyDeviceInfoList(deviceInfo);
78  return false;
79  }
80 
81  detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
82  length = requiredLength;
83  SP_DEVINFO_DATA DevInfoData;
84  DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
85  bResult = SetupDiGetDeviceInterfaceDetailA(deviceInfo, &interfaceData, detailData, length, &requiredLength, &DevInfoData);
86 
87  if (!bResult)
88  {
89  LocalFree(detailData);
90  SetupDiDestroyDeviceInfoList(deviceInfo);
91  return false;
92  }
93 
94  unsigned char serialNumber[256];
95  char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#');
96  if (!ptrStart)
97  continue;
98  ptrStart = strchr(ptrStart+1, '#');
99  if (!ptrStart)
100  continue;
101  ptrEnd = strchr(ptrStart+1, '#');
102  if (!ptrEnd)
103  continue;
104 
105  strncpy((char*)serialNumber, ptrStart+1, ptrEnd-ptrStart-1);
106  serialNumber[ptrEnd-ptrStart-1] = '\0';
107 
108  current.setPortName(detailData->DevicePath);
109 
110  int id = 0;
111  sscanf((const char *)serialNumber, "%X", &id);
112  current.setDeviceId((uint32_t) id);
113 
114  ports.push_back(current);
115  }
116  }
117 
118  SetupDiDestroyDeviceInfoList(deviceInfo);
119  return true;
120 #else
121  XsLibUsb libUsb;
122  libusb_context *context;
123  int result = libUsb.init(&context);
124  if (result != LIBUSB_SUCCESS)
125  return false;
126 
127  libusb_device **deviceList;
128  ssize_t deviceCount = libUsb.get_device_list(context, &deviceList);
129  for (ssize_t i = 0; i < deviceCount; i++)
130  {
131  libusb_device *device = deviceList[i];
132  libusb_device_descriptor desc;
133  result = libUsb.get_device_descriptor(device, &desc);
134  if (result != LIBUSB_SUCCESS)
135  continue;
136 
137  if (desc.idVendor != XSENS_VENDOR_ID && desc.idVendor != ATMEL_VENDOR_ID)
138  continue;
139 
140  libusb_device_handle *handle;
141  result = libUsb.open(device, &handle);
142  if (result != LIBUSB_SUCCESS)
143  {
144  libUsb.unref_device(device);
145  continue;
146  }
147 
148  unsigned char serialNumber[256];
149  result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256);
150 
151  if (desc.idVendor == ATMEL_VENDOR_ID && desc.idProduct == ATMEL_BORROWED_PRODUCT_ID)
152  {
153  unsigned char productName[256];
154  result = libUsb.get_string_descriptor_ascii(handle, desc.iProduct, productName, 256);
155 
156  if (strcmp("Xsens COM port", (const char *)productName) != 0)
157  {
158  libUsb.close(handle);
159  continue;
160  }
161  }
162 
163  libusb_config_descriptor *configDesc;
164  result = libUsb.get_active_config_descriptor(device, &configDesc);
165  if (result != LIBUSB_SUCCESS)
166  continue;
167 
168  bool kernelActive = false;
169  for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) {
170  int res = libUsb.kernel_driver_active(handle, ifCount);
171  kernelActive |= (res == 1);
172 
173  }
174 
175  libUsb.free_config_descriptor(configDesc);
176 
177  if (!kernelActive)
178  {
179  char name[256];
180  sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device),
181  libUsb.get_device_address(device));
182  current.setPortName(name);
183 
184  int id = 0;
185  sscanf((const char *)serialNumber, "%d", &id);
186  current.setDeviceId((uint32_t) id);
187  ports.push_back(current);
188  }
189  else
190  {
191  JLDEBUG(gJournal, "Kernel driver active on USB" <<
192  libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) <<
193  " device " << serialNumber);
194 
195  }
196  libUsb.close(handle);
197  }
198  libUsb.free_device_list(deviceList, 1);
199  libUsb.exit(context);
200  return true;
201 #endif
202 }
#define XSENS_VENDOR_ID
libUSB_close close
Definition: xslibusb.h:48
libUSB_unref_device unref_device
Definition: xslibusb.h:53
libUSB_open open
Definition: xslibusb.h:47
libUSB_init init
Definition: xslibusb.h:45
libUSB_free_config_descriptor free_config_descriptor
Definition: xslibusb.h:57
#define ATMEL_BORROWED_PRODUCT_ID
int BOOL
Definition: xstypedefs.h:62
libUSB_get_string_descriptor_ascii get_string_descriptor_ascii
Definition: xslibusb.h:64
unsigned char uint8_t
Definition: rptypes.h:43
libUSB_free_device_list free_device_list
Definition: xslibusb.h:63
bool xsEnumerateUsbDevices(XsPortInfoList &ports)
Enumerate Xsens USB devices.
#define FALSE
Definition: jmorecfg.h:227
libUSB_get_device_list get_device_list
Definition: xslibusb.h:62
libUSB_get_device_descriptor get_device_descriptor
Definition: xslibusb.h:61
libUSB_kernel_driver_active kernel_driver_active
Definition: xslibusb.h:49
libUSB_exit exit
Definition: xslibusb.h:46
#define JLDEBUG(...)
GLuint GLsizei GLsizei * length
Definition: glext.h:3900
GLuint const GLchar * name
Definition: glext.h:3891
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS 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:191
libUSB_get_device_address get_device_address
Definition: xslibusb.h:60
GLuint res
Definition: glext.h:6298
unsigned __int32 uint32_t
Definition: rptypes.h:49
libUSB_get_bus_number get_bus_number
Definition: xslibusb.h:58
libUSB_get_active_config_descriptor get_active_config_descriptor
Definition: xslibusb.h:56
#define XsPortInfoList
#define ATMEL_VENDOR_ID



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019