Main MRPT website > C++ reference for MRPT 1.5.6
usbinterface.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 <xsens/xsthread.h>
10 #include <xsens/xsportinfo.h>
11 #include "usbinterface.h"
12 #include <errno.h>
13 
14 #ifdef USE_WINUSB
15 # include "xswinusb.h"
16 #else
17 # include "xslibusb.h"
18 #endif
19 
20 #ifndef _WIN32
21 # include <string.h> // strcpy
22 #else
23 # include <winbase.h>
24 # include <io.h>
25 #endif
26 
27 #ifndef _CRT_SECURE_NO_DEPRECATE
28 # define _CRT_SECURE_NO_DEPRECATE
29 # ifdef _WIN32
30 # pragma warning(disable:4996)
31 # endif
32 #endif
33 
34 /*! \brief Private object for UsbInterface */
36 {
37 public:
38  #ifdef LOG_RX_TX
39  XsFileHandle* rx_log;
40  XsFileHandle* tx_log;
41  #endif
42 
43  //! The time at which an operation will end in ms, used by several functions.
45  //! The last result of an operation
47  /*! The default timeout value to use during blocking operations.
48  A value of 0 means that all operations become non-blocking.
49  */
51 
56  char m_portname[256];
57 
58 #ifdef USE_WINUSB
59  XsWinUsb m_winUsb;
60  WINUSB_INTERFACE_HANDLE m_usbHandle[2];
61  uint8_t m_bulkInPipe, m_bulkOutPipe, m_interruptPipe, m_deviceSpeed;
62  uint16_t m_bulkInPipePacketSize;
64 
65  XsThreadId m_threadId;
66  XsThread m_threadHandle;
67  static const int m_oCount = MAXIMUM_WAIT_OBJECTS-1;
68  OVERLAPPED m_overlapped[m_oCount];
69  XsByteArray m_varBuffer;
70  static const int m_fixedBufferSize = 8192;
71  static const int m_fastPolicyThreshold = 3;
72  uint8_t m_fixedBuffer[m_oCount][m_fixedBufferSize];
73  //int m_offset;
74  CRITICAL_SECTION m_mutex;
75  HANDLE m_quitEvent;
76  volatile bool m_running;
77  HANDLE m_waitEvents[m_oCount];
78  int m_readIdx;
79 
80  void threadFunc();
81 
82 #else
83  libusb_device_handle *m_deviceHandle;
84  /*! \brief A context manager for libusb
85 
86  For predictable operation with libusb, it is recommended to use at least one context per library.
87  */
88  class UsbContext {
89  public:
90  /*! \brief Create the USB context
91  */
93  {
95  //libusb_set_debug(m_usbContext, 3);
96  }
97 
98  /*! \brief Destroy the USB context */
100  {
102  }
103  libusb_context *m_usbContext; // needed for proper use of libusb
105  };
106 
107  // JLBC for MRPT: Avoid crashes in apps exit, even when XSens code is not called:
108  // -> Converted into a singleton:
109  //Was: static UsbContext m_contextManager;
111  {
112  static UsbContext obj;
113  return obj;
114  }
115 
116  /*! \brief Map a libusb_error to XsResultValue
117 
118  \a param libusbError [in] the result code to convert
119  \a param hint give a hint for the code to return when in doubt
120  */
122  {
123  switch (libusbError) {
124  case LIBUSB_SUCCESS:
125  return XRV_OK;
126 
127  case LIBUSB_ERROR_IO:
128  return hint;
129 
130  case LIBUSB_ERROR_INVALID_PARAM:
131  return XRV_INVALIDPARAM;
132 
133  case LIBUSB_ERROR_ACCESS:
134  return hint;
135 
136  case LIBUSB_ERROR_NO_DEVICE:
137  return XRV_NOPORTOPEN;
138 
139  case LIBUSB_ERROR_NOT_FOUND:
140  return XRV_NOTFOUND;
141 
142  case LIBUSB_ERROR_BUSY:
143  return XRV_INVALIDOPERATION;
144 
145  case LIBUSB_ERROR_TIMEOUT:
146  return XRV_TIMEOUT;
147 
148  case LIBUSB_ERROR_OVERFLOW:
149  return hint;
150 
151  case LIBUSB_ERROR_PIPE:
152  return hint;
153 
154  case LIBUSB_ERROR_INTERRUPTED:
155  return XRV_UNEXPECTEDMSG;
156 
157  case LIBUSB_ERROR_NO_MEM:
158  return XRV_OUTOFMEMORY;
159 
160  case LIBUSB_ERROR_NOT_SUPPORTED:
161  return XRV_NOTIMPLEMENTED;
162 
163  case LIBUSB_ERROR_OTHER:
164  return hint;
165  }
166  return hint;
167  }
168 
169  /*! \brief Convert a libusb error to a human-readable string */
170  const char *libusbErrorToString(int libusbError)
171  {
172  switch (libusbError) {
173  case LIBUSB_SUCCESS:
174  return "LIBUSB_SUCCESS";
175 
176  case LIBUSB_ERROR_IO:
177  return "LIBUSB_ERROR_IO";
178 
179  case LIBUSB_ERROR_INVALID_PARAM:
180  return "LIBUSB_ERROR_INVALID_PARAM";
181 
182  case LIBUSB_ERROR_ACCESS:
183  return "LIBUSB_ERROR_ACCESS";
184 
185  case LIBUSB_ERROR_NO_DEVICE:
186  return "LIBUSB_ERROR_NO_DEVICE";
187 
188  case LIBUSB_ERROR_NOT_FOUND:
189  return "LIBUSB_ERROR_NOT_FOUND";
190 
191  case LIBUSB_ERROR_BUSY:
192  return "LIBUSB_ERROR_BUSY";
193 
194  case LIBUSB_ERROR_TIMEOUT:
195  return "LIBUSB_ERROR_TIMEOUT";
196 
197  case LIBUSB_ERROR_OVERFLOW:
198  return "LIBUSB_ERROR_OVERFLOW";
199 
200  case LIBUSB_ERROR_PIPE:
201  return "LIBUSB_ERROR_PIPE";
202 
203  case LIBUSB_ERROR_INTERRUPTED:
204  return "LIBUSB_ERROR_INTERRUPTED";
205 
206  case LIBUSB_ERROR_NO_MEM:
207  return "LIBUSB_ERROR_NO_MEM";
208 
209  case LIBUSB_ERROR_NOT_SUPPORTED:
210  return "LIBUSB_ERROR_NOT_SUPPORTED";
211 
212  case LIBUSB_ERROR_OTHER:
213  return "LIBUSB_ERROR_OTHER";
214  }
215  return "Unknown";
216  }
217 #endif
218 };
219 
220 #ifdef USE_WINUSB
221 DWORD usbReadThreadFunc(void* obj)
222 {
224  d->m_running = true;
226  xsNameThisThread("USB reader");
227  try {
228  d->threadFunc();
229  d->m_running = false;
230  } catch(...)
231  {
232  xsNameThisThread("Crashed USB reader");
233  d->m_running = false;
234  XsTime::msleep(10000);
235  }
236  return 0;
237 }
238 
239 void UsbInterfacePrivate::threadFunc()
240 {
241  HANDLE handles[1+m_oCount];
242  handles[0] = m_quitEvent;
243  handles[m_oCount] = m_waitEvents[m_oCount-1];
244  //= { m_quitEvent, m_waitEvents[0], m_waitEvents[1] };
245 
246  // start first read operation
247  for (m_readIdx = 0 ; m_readIdx < (m_oCount-1); ++m_readIdx)
248  {
249  handles[m_readIdx+1] = m_waitEvents[m_readIdx];
250  //m_readIdx = 0;
251  m_overlapped[m_readIdx] = OVERLAPPED();
252  ::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534
253  m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
254  m_winUsb.ReadPipe(m_usbHandle[1],
255  m_bulkInPipe,
256  m_fixedBuffer[m_readIdx],
257  (ULONG)m_fixedBufferSize,
258  0,
259  &m_overlapped[m_readIdx]); //lint !e534
260  }
261  int fastCount = 0;
262  //m_readIdx = 1;
263  bool policyFast = false;
264  bool run = true;
265  while (run)
266  {
267  // start follow-up read operation
268  m_overlapped[m_readIdx] = OVERLAPPED();
269  ::ResetEvent(m_waitEvents[m_readIdx]); //lint !e534
270  m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
271  m_winUsb.ReadPipe(m_usbHandle[1],
272  m_bulkInPipe,
273  m_fixedBuffer[m_readIdx],
274  (ULONG)m_fixedBufferSize,
275  0,
276  &m_overlapped[m_readIdx]); //lint !e534
277  m_readIdx = (m_readIdx + 1) % m_oCount;
278  int64_t tBegin = XsTime_timeStampNow(0);
279  DWORD waitResult = ::WaitForMultipleObjects(1+m_oCount, handles, FALSE, INFINITE);
280 #if 0 // not sure if this causes problems, but it should help in catching up
281  int64_t tEnd = XsTime_timeStampNow(0);
282  switch (tEnd - tBegin)
283  {
284  case 0:
285  if (++fastCount > m_fastPolicyThreshold && !policyFast)
286  {
287  policyFast = true;
288  // set fast policy
289  UCHAR enable = TRUE;
290  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
291  }
292  break;
293 
294  case 1:
295  if (fastCount)
296  --fastCount;
297  if (policyFast && fastCount <= m_fastPolicyThreshold)
298  {
299  // reset policy
300  policyFast = false;
301  UCHAR enable = FALSE;
302  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
303  }
304  break;
305 
306  default:
307  fastCount = 0;
308  if (policyFast)
309  {
310  // reset policy
311  policyFast = false;
312  UCHAR enable = FALSE;
313  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
314  }
315  break;
316  }
317 #endif
318 
319  // handle data
320  switch (waitResult)
321  {
322  case WAIT_TIMEOUT:
323  case WAIT_FAILED:
324  case WAIT_OBJECT_0:
325  run = false;
326  break;
327 
328  default:
329  if (waitResult >= WAIT_ABANDONED_0)
330  {
331  JLDEBUG(gJournal, "WFMO abandoned: " << (waitResult - WAIT_OBJECT_0));
332  break;
333  }
334 
335 #ifndef XSENS_RELEASE
336  JLDEBUG(gJournal, "WFMO trigger: " << (waitResult - WAIT_OBJECT_0));
337 #endif
338  {
339  // put data into buffer
340  int idx = m_readIdx;
341  DWORD dataRead = 0;
342  if (!m_winUsb.GetOverlappedResult(m_usbHandle[0], &m_overlapped[idx], &dataRead, FALSE))
343  {
344  // error
345  DWORD err = ::GetLastError();
346  switch (err)
347  {
348  case ERROR_SEM_TIMEOUT:
349  case ERROR_IO_INCOMPLETE:
350  //JLDEBUG(gJournal, "m_winUsb.GetOverlappedResult resulted in acceptable windows error " << err);
351  break;
352 
353  default:
354  JLALERT(gJournal, "m_winUsb.GetOverlappedResult resulted in windows error " << err);
355  run = false;
356  break;
357  }
358  //assert (err == ERROR_IO_INCOMPLETE);
359  }
360  else
361  {
362  // append unread data to var buffer
363  JLTRACE(gJournal, "m_winUsb.GetOverlappedResult resulted in " << dataRead << " bytes being read");
364  XsByteArray ref(&m_fixedBuffer[idx][0], dataRead, XSDF_None);
365  ::EnterCriticalSection(&m_mutex);
366  m_varBuffer.append(ref);
367  ::LeaveCriticalSection(&m_mutex);
368  }
369  } break;
370  }
371  }
372 }
373 
374 #else
375 //UsbInterfacePrivate::UsbContext UsbInterfacePrivate::m_contextManager; // JLBC for MRPT: Removed due to factoring as singleton
376 // => UsbInterfacePrivate::getContextManager()
377 #endif
378 
379 /*! \class UsbInterface
380  \brief An IoInterface for dealing specifically with Xsens USB devices
381 */
382 
383 /*! \brief Default constructor, initializes all members to their default values.
384 */
386  d(new UsbInterfacePrivate)
387 {
388  d->m_lastResult = XRV_OK;
389  d->m_timeout = 20;
390  d->m_endTime = 0;
391  d->m_dataInEndPoint = -1;
392  d->m_dataOutEndPoint = -1;
393  d->m_deviceHandle = 0;
394  d->m_portname[0] = 0;
395  #ifdef LOG_RX_TX
396  d->rx_log = NULL;
397  d->tx_log = NULL;
398  #endif
399  #ifdef USE_WINUSB
400  d->m_threadHandle = INVALID_HANDLE_VALUE;
401  ::InitializeCriticalSection(&d->m_mutex);
402  d->m_usbHandle[0] = 0;
403  d->m_usbHandle[1] = 0;
404  for (int i = 0; i < d->m_oCount; ++i)
405  {
406  d->m_waitEvents[i] = ::CreateEvent(NULL, TRUE, FALSE, NULL);
407  ::ResetEvent(d->m_waitEvents[i]);
408  }
409  d->m_quitEvent = ::CreateEvent(NULL, TRUE, FALSE, NULL);
410  ::ResetEvent(d->m_quitEvent);
411  d->m_readIdx = 0;
412 // d->m_offset = 0;
413  #endif
414 }
415 
416 //! Destructor, de-initializes, frees memory allocated for buffers, etc.
418 {
419  try {
420  closeUsb(); //lint !e534
421 #ifdef USE_WINUSB
422  ::CloseHandle(d->m_quitEvent); //lint !e534
423  for (int i = 0; i < d->m_oCount; ++i)
424  ::CloseHandle(d->m_waitEvents[i]); //lint !e534
425  ::DeleteCriticalSection(&d->m_mutex);
426 #endif
427  delete d;
428  } catch(...)
429  {}
430 }
431 
432 /*! \brief Close the USB communication port.
433 */
435 {
436  return closeUsb();
437 }
438 
439 /*! \brief Close the USB communication port.
440  \returns XRV_OK if the port was closed successfully
441  \note Linux:\n
442  If a kernel driver was detached when communication with the device started,
443  attach it again. No guarantee is given that udev will pick up on it though.
444 */
446 {
447  //lint --e{534}
448 #ifdef LOG_RX_TX
449  if (d->rx_log != NULL)
450  fclose(d->rx_log);
451  if (d->tx_log != NULL)
452  fclose(d->tx_log);
453  d->rx_log = NULL;
454  d->tx_log = NULL;
455 #endif
456  if (!isOpen())
457  return d->m_lastResult = XRV_NOPORTOPEN;
458 
459  d->m_lastResult = XRV_OK;
460 #ifdef USE_WINUSB
461  if (d->m_threadHandle != INVALID_HANDLE_VALUE)
462  {
463  while (d->m_running)
464  {
465  ::SetEvent(d->m_quitEvent);
466  XsTime::msleep(10);
467  }
468  ::CloseHandle(d->m_threadHandle);
469  }
470 
471  flushData();
472  if(d->m_usbHandle[0]) {
473  d->m_winUsb.Free(d->m_usbHandle[0]);
474  d->m_usbHandle[0] = NULL;
475  }
476  if(d->m_usbHandle[1]) {
477  d->m_winUsb.Free(d->m_usbHandle[1]);
478  d->m_usbHandle[1] = NULL;
479  }
480  if (d->m_deviceHandle) {
481  CloseHandle(d->m_deviceHandle);
482  d->m_deviceHandle = NULL;
483  }
484 #else
485  flushData();
487  for (int i = 0; i < d->m_interfaceCount; i++) {
488  int result = LIBUSB_ERROR_OTHER;
489  while (result != LIBUSB_SUCCESS) {
491  if (result == LIBUSB_SUCCESS) {
493  }
494  }
495  }
496 
498  d->m_deviceHandle = NULL;
499 
501  d->m_interface = -1;
502  d->m_dataInEndPoint = -1;
503  d->m_dataOutEndPoint = -1;
504 #endif
505  d->m_endTime = 0;
506 
507  return d->m_lastResult;
508 }
509 
510 //////////////////////////////////////////////////////////////////////////////////////////
511 // Flush all data to be transmitted / received.
513 {
514  //lint --e{534}
515  d->m_lastResult = XRV_OK;
516 #ifdef USE_WINUSB
517  if(d->m_usbHandle[0]) {
518  d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkInPipe);
519  d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkInPipe);
520  d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkOutPipe);
521  d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkOutPipe);
522  }
523  if(d->m_usbHandle[1]) {
524  d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkInPipe);
525  d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkInPipe);
526  d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkOutPipe);
527  d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkOutPipe);
528  }
529 #else
530  unsigned char flushBuffer[256];
531  int actual;
532  for (int i = 0; i < 64; ++i) {
533  if (UsbInterfacePrivate::getContextManager().m_libUsb.bulk_transfer(d->m_deviceHandle, d->m_dataInEndPoint|LIBUSB_ENDPOINT_IN,
534  flushBuffer, sizeof(flushBuffer), &actual, 1) != LIBUSB_SUCCESS)
535  break;
536  if (actual == 0)
537  break;
538  }
539 #endif
540  d->m_endTime = 0;
541  return d->m_lastResult;
542 }
543 
544 //! Return the error code of the last operation.
546 {
547  return d->m_lastResult;
548 }
549 
550 //! Return the current timeout value
552 {
553  return d->m_timeout;
554 }
555 
556 //! Return whether the USB communication port is open or not.
557 bool UsbInterface::isOpen (void) const
558 {
559  return d->m_deviceHandle != NULL;
560 }
561 
562 /*! \brief Open a communication channel to the given USB port name. */
564 {
565  d->m_endTime = 0;
566 
567 #ifdef USE_WINUSB
568  JLDEBUG(gJournal, "Open usb port " << portInfo.portName().toStdString());
569 #else
570  JLDEBUG(gJournal, "Open usb port " << portInfo.usbBus() << ":" << portInfo.usbAddress());
571 #endif
572 
573  if (isOpen())
574  {
575  JLALERT(gJournal, "Port " << portInfo.portName().toStdString() << " already open");
576  return (d->m_lastResult = XRV_ALREADYOPEN);
577  }
578 
579 #ifdef USE_WINUSB
580  d->m_deviceHandle = CreateFileA(portInfo.portName().c_str(),
581  GENERIC_WRITE | GENERIC_READ,
582  FILE_SHARE_WRITE | FILE_SHARE_READ,
583  NULL,
584  OPEN_EXISTING,
585  FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED,
586  NULL);
587 
588  if (d->m_deviceHandle == INVALID_HANDLE_VALUE)
589  {
590  d->m_deviceHandle = NULL;
591  return (d->m_lastResult = XRV_PORTNOTFOUND);
592  }
593 
594  BOOL result = FALSE;
595  UCHAR speed = 0;
596  ULONG length = 0;
597  USB_INTERFACE_DESCRIPTOR interfaceDescriptor = {0,0,0,0,0,0,0,0,0};
598  WINUSB_PIPE_INFORMATION pipeInfo;
599 
600  result = d->m_winUsb.Initialize(d->m_deviceHandle, &d->m_usbHandle[0]);
601  if (result)
602  {
603  result = d->m_winUsb.GetAssociatedInterface(d->m_usbHandle[0],0,&d->m_usbHandle[1]);
604  }
605  else
606  {
607 #ifdef XSENS_DEBUG
608  DWORD err = GetLastError();
609  assert(result);
610 #endif
611  return (d->m_lastResult = XRV_ERROR);
612  }
613 
614  for (int k = 0; k<2;k++)
615  {
616  if(result)
617  {
618  assert(d->m_usbHandle[k] != 0);
619  length = sizeof(UCHAR);
620  result = d->m_winUsb.QueryDeviceInformation(d->m_usbHandle[k],
621  DEVICE_SPEED,
622  &length,
623  &speed);
624  }
625 
626  if(result)
627  {
628  d->m_deviceSpeed = speed;
629  result = d->m_winUsb.QueryInterfaceSettings(d->m_usbHandle[k],
630  0,
631  &interfaceDescriptor);
632  }
633  if(result)
634  {
635  for(int i=0;i<interfaceDescriptor.bNumEndpoints;i++)
636  {
637  result = d->m_winUsb.QueryPipe(d->m_usbHandle[k],
638  0,
639  (UCHAR) i,
640  &pipeInfo);
641 
642  if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
643  USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId))
644  {
645  d->m_bulkInPipe = pipeInfo.PipeId;
646  d->m_bulkInPipePacketSize =
647  pipeInfo.MaximumPacketSize;
648  }
649  else if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
650  USB_ENDPOINT_DIRECTION_OUT(pipeInfo.PipeId))
651  {
652  d->m_bulkOutPipe = pipeInfo.PipeId;
653  }
654  else if(pipeInfo.PipeType == UsbdPipeTypeInterrupt)
655  {
656  d->m_interruptPipe = pipeInfo.PipeId;
657  }
658  else
659  {
660  result = FALSE;
661  break;
662  }
663  }
664  }
665  }
666 
667  setTimeout(0); //lint !e534
668  flushData(); //lint !e534
669 
670  sprintf(d->m_portname, "%s", portInfo.portName().c_str());
671 
672 // d->m_offset = 0;
673  ::ResetEvent(&d->m_quitEvent); //lint !e534
674  d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId);
675  if (d->m_threadHandle == XSENS_INVALID_THREAD)
676  {
677 #ifdef XSENS_DEBUG
678  assert(0);
679 #endif
680  return (d->m_lastResult = XRV_ERROR);
681  }
682 
683 #else // !USE_WINUSB
684  libusb_device **deviceList;
686  if (listLength < 0)
687  return d->m_lastResult = d->libusbErrorToXrv((int)listLength);
688 
689  // "USBxxx:yyy"
690  uint8_t bus = XsPortInfo_usbBus(&portInfo);
692 
693  XsResultValue xrv = XRV_OK;
694  int result;
695  libusb_device *device = NULL;
696  for (int i = 0; i < listLength && device == NULL; ++i) {
697  libusb_device *dev = deviceList[i];
699  continue;
700 
701  libusb_device_descriptor desc;
703  if (result != LIBUSB_SUCCESS)
704  break;
705 
706  libusb_config_descriptor *configDesc;
708  if (result != LIBUSB_SUCCESS)
709  break;
710 
711  d->m_interface = -1;
712  d->m_interfaceCount = configDesc->bNumInterfaces;
713  // find the bulk transfer endpoints
714  for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces && d->m_interface == -1; ++ifCount) {
715  for (uint8_t altsettingCount = 0; altsettingCount < configDesc->interface[ifCount].num_altsetting; altsettingCount++) {
716  const libusb_endpoint_descriptor *endpoints = configDesc->interface[ifCount].altsetting[altsettingCount].endpoint;
717  int inEndpoint = -1, outEndpoint = -1;
718  for (uint8_t i = 0; i < configDesc->interface[ifCount].altsetting[altsettingCount].bNumEndpoints; i++) {
719  if ((endpoints[i].bmAttributes&LIBUSB_TRANSFER_TYPE_MASK) != LIBUSB_TRANSFER_TYPE_BULK)
720  continue;
721 
722  switch (endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_DIR_MASK) {
723  case LIBUSB_ENDPOINT_IN:
724  inEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK;
725  break;
726 
727  case LIBUSB_ENDPOINT_OUT:
728  outEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK;
729  break;
730  }
731 
732  }
733 
734  if (outEndpoint == -1 || inEndpoint == -1)
735  continue;
736 
737  d->m_interface = ifCount;
738  d->m_dataOutEndPoint = outEndpoint;
739  d->m_dataInEndPoint = inEndpoint;
740  }
741  }
742  if (d->m_interface == -1) {
744  break;
745  }
746 
749  device = dev;
750  result = LIBUSB_SUCCESS;
751  }
752 
754  if (result != LIBUSB_SUCCESS) {
756  return d->m_lastResult = d->libusbErrorToXrv(result);
757  }
758 
759  if (xrv != XRV_OK) {
761  return d->m_lastResult = xrv;
762  }
763 
764  libusb_device_handle *handle;
765  result = UsbInterfacePrivate::getContextManager().m_libUsb.open(device, &handle);
766  if (result != LIBUSB_SUCCESS) {
768  return d->m_lastResult = d->libusbErrorToXrv(result);
769  }
770 
771  // be rude and claim all interfaces
772  for (int i = 0; i < d->m_interfaceCount; i++) {
774  if (result > 0)
776  if (result == LIBUSB_SUCCESS)
778  if (result != LIBUSB_SUCCESS) {
779  for (int j = 0; j < i; j++) {
780  while (result != LIBUSB_SUCCESS) {
783  }
784  }
785 
788  return d->m_lastResult = d->libusbErrorToXrv(result);
789  }
790  }
791 
792  d->m_deviceHandle = handle;
793  sprintf(d->m_portname, "%s", portInfo.portName().c_str());
794 
795  flushData();
796 
797 #endif // !USE_WINUSB
798  JLDEBUG(gJournal, "USB Port opened");
799  return (d->m_lastResult = XRV_OK);
800 }
801 
802 /*! \brief Read data from the USB port and put it into the data buffer.
803  \details This function reads up to \a maxLength bytes from the port (non-blocking) and
804  puts it into the \a data buffer.
805  \param maxLength The maximum amount of data read.
806  \param data The buffer that will store the received data.
807  \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be
808  returned. Check data.size() for the number of bytes that were read.
809 */
811 {
812  XsSize length = 0;
813  data.setSize(maxLength);
815  data.pop_back(maxLength - length);
816  return res;
817 }
818 
819 /*! \brief Read data from the serial port and put it into the data buffer.
820  \details This function reads up to \a maxLength bytes from the USB port (non-blocking) and
821  puts it into the \a data buffer.
822  \param maxLength The maximum number of bytes to read.
823  \param data Pointer to a buffer that will store the received data.
824  \param length The number of bytes placed into \c data.
825  \returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be
826  returned. Check *length for the number of bytes that were read.
827 */
829 {
830  JLTRACE(gJournal, "maxLength=" << maxLength << ", data=0x" << data << ", length=0x" << length);
831  XsSize ln;
832  if (length == NULL)
833  length = &ln;
834 
835  if (!isOpen())
836  return (d->m_lastResult = XRV_NOPORTOPEN);
837 
838 #ifdef USE_WINUSB
839  XsSize remaining = 0;
840  ::EnterCriticalSection(&d->m_mutex);
841  remaining = *length = d->m_varBuffer.size();
842  if (*length > maxLength)
843  *length = maxLength;
844  if (*length)
845  {
846  memcpy(data, d->m_varBuffer.data(), *length);
847  d->m_varBuffer.erase(0, *length);
848  remaining = d->m_varBuffer.size();
849  }
850  ::LeaveCriticalSection(&d->m_mutex);
851  JLTRACE(gJournal, "returned success, read " << *length << " of " << maxLength << " bytes, first: " << JLHEXLOG(((char*)data)[0]) << ", " << remaining << " remaining in buffer");
852 #else
853  int actual = 0;
854  JLTRACE(gJournal, "starting bulk read, timeout = " << d->m_timeout);
855  int res = UsbInterfacePrivate::getContextManager().m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataInEndPoint|LIBUSB_ENDPOINT_IN), (unsigned char *)data, maxLength, &actual, d->m_timeout);
856  JLTRACE(gJournal, "bulk read returned: " << d->libusbErrorToString(res) << ". " << actual << " bytes received");
857  if ((res != LIBUSB_SUCCESS && res != LIBUSB_ERROR_TIMEOUT) || (res == LIBUSB_ERROR_TIMEOUT && actual <= 0))
858  return d->m_lastResult = d->libusbErrorToXrv(res);
859 
860  *length = actual;
861 #endif
862 
863 #ifdef LOG_RX_TX
864  if (*length > 0)
865  {
866  if (d->rx_log == NULL)
867  {
868  char fname[XS_MAX_FILENAME_LENGTH];
869  sprintf(fname,"rx_USB%03u_%03u.log", usbBus(), usbAddress());
870  d->rx_log = fopen(fname,"wb");
871  }
872  fwrite(data,1,*length,d->rx_log);
873 #ifdef LOG_RX_TX_FLUSH
874  fflush(d->rx_log);
875 #endif
876  }
877 #endif
878 
879  return (d->m_lastResult = XRV_OK);
880 }
881 
882 /*! \brief Set the default timeout value to use in blocking operations.
883  \details This function sets the value of m_timeout. There is no infinity value. The value 0
884  means that the default value is used.
885  \param ms The desired timeout in milliseconds
886  \returns XRV_OK if the timeout value was successfully updated
887 */
889 {
890 #ifdef USE_WINUSB
891  JLDEBUG(gJournal, "Request to set timeout to " << ms << " ms overridden, setting to 0 ms instead");
892  ms = 0; // no timeout ever
893  UCHAR enable = FALSE;
894 
895  d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
896  d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, PIPE_TRANSFER_TIMEOUT, sizeof(ULONG), &ms); //lint !e534
897 
898  d->m_timeout = ms;
899 #else
900  JLDEBUG(gJournal, "Setting timeout to " << ms);
901  if (ms == 0)
902  d->m_timeout = 1;
903  else
904  d->m_timeout = ms;
905 #endif
906  return (d->m_lastResult = XRV_OK);
907 }
908 
909 /*! \brief Sets the RAWIO mode of the USB interface
910  \note Only applies to WinUSB implementations
911  \param enable : If true will enable RAW IO mode
912 */
913 void UsbInterface::setRawIo(bool enable)
914 {
915  JLDEBUG(gJournal, "Setting RAWIO mode to " << enable);
916 
917 #ifdef USE_WINUSB
918  enable = false; // never use raw IO
919  UCHAR rawIo = (UCHAR)enable;
920  d->m_winUsb.SetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, sizeof(UCHAR), &rawIo); //lint !e534
921 #else
922  (void)enable;
923 #endif
924  d->m_lastResult = XRV_OK;
925 }
926 
927 /*! \brief Retrieves the state of the RAWIO mode of the USB interface
928  \returns true if raw IO mode is enabled
929  \note Only applies to WinUSB implementations
930 */
932 {
933 #ifdef USE_WINUSB
934  UCHAR rawIo = 0;
935  ULONG someSize = sizeof(UCHAR);
936  d->m_winUsb.GetPipePolicy(d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, &someSize, &rawIo); //lint !e534
937  return (rawIo != 0);
938 #else
939  return false;
940 #endif
941 }
942 
943 /*! \brief Wait for data to arrive or a timeout to occur.
944  \details The function waits until \c maxLength data is available or until a timeout occurs.
945  The function returns success if data is available or XsResultValue::TIMEOUT if a
946  timeout occurred. A timeout value of 0 indicates that the default timeout stored
947  in the class should be used.
948  \param maxLength The maximum number of bytes to wait for
949  \param data A buffer that will be filled with the read data. It must be able to contain at
950  least \a maxLength bytes.
951  \param length An optional pointer to storage for the actual number of bytes read.
952  \returns XRV_OK if the requested data was read
953 */
955 {
956  JLTRACE(gJournal, "timeout=" << d->m_timeout << ", data=" << data << ", length=" << length);
957  uint32_t timeout = d->m_timeout;
958 
959  char *bdata = (char *)data;
960 
961  XsSize ln;
962  if (length == NULL)
963  length = &ln;
964  uint32_t eTime = XsTime::getTimeOfDay() + timeout;
965  XsSize newLength = 0;
966 
967  *length = 0;
968  while ((*length < maxLength) && (XsTime::getTimeOfDay() <= eTime))
969  {
970  if (readData(maxLength - *length, bdata + *length, &newLength))
971  return d->m_lastResult;
972  *length += newLength;
973  }
974  JLTRACE(gJournal, "read " << length[0] << " of " << maxLength << " bytes");
975 
976  if (length[0] < maxLength)
977  return (d->m_lastResult = XRV_TIMEOUT);
978  else
979  return (d->m_lastResult = XRV_OK);
980 }
981 
982 /*! \brief Write the data to the USB port.
983  \details The function writes the given data to the connected USB port.
984  The default timeout is respected in this operation.
985  \param data The data to be written
986  \param written An optional pointer to storage for the actual number of bytes that were written
987  \returns XRV_OK if the data was successfully written
988  \sa writeData(const XsSize, const void *, XsSize*)
989 */
991 {
992  return writeData(data.size(), data.data(), written);
993 }
994 
995 /*! \brief Write the data to the USB port.
996  \details The function writes the given data to the connected USB port.
997  The default timeout is respected in this operation.
998  \param length The number of bytes to write.
999  \param data A pointer to a memory buffer that contains the bytes to send
1000  \param written An optional pointer to storage for the actual number of bytes that were written
1001  \returns XRV_OK if the data was successfully written
1002  \sa writeData(const XsByteArray&, XsSize*)
1003 */
1005 {
1006  XsSize bytes;
1007  if (written == NULL)
1008  written = &bytes;
1009 
1010  if (!isOpen())
1011  return (d->m_lastResult = XRV_NOPORTOPEN);
1012 
1013  *written = 0;
1014 
1015 #ifdef USE_WINUSB
1016  ULONG dataWritten;
1017  d->m_winUsb.WritePipe(d->m_usbHandle[1],
1018  d->m_bulkOutPipe,
1019  (uint8_t*)data,
1020  (ULONG)length,
1021  &dataWritten,
1022  NULL); //lint !e534
1023 
1024  *written = dataWritten;
1025 #else
1026  *written = 0;
1027  while (*written < length)
1028  {
1029  int actual;
1030  int result = UsbInterfacePrivate::getContextManager().m_libUsb.bulk_transfer(d->m_deviceHandle, (d->m_dataOutEndPoint|LIBUSB_ENDPOINT_OUT), (unsigned char *)data, length, &actual, 0);
1031  *written += actual;
1032  if (result != LIBUSB_SUCCESS)
1033  {
1034  JLALERT(gJournal, "bulk write failed: " << d->libusbErrorToString(result) << ". " << actual << " bytes sent");
1035  return (d->m_lastResult = d->libusbErrorToXrv(result));
1036  }
1037  }
1038 
1039 #endif
1040 
1041 #ifdef LOG_RX_TX
1042  if (*written > 0)
1043  {
1044  if (d->tx_log == NULL)
1045  {
1046  char fname[XS_MAX_FILENAME_LENGTH];
1047  sprintf(fname,"tx_USB%03u_%03u.log", usbBus(), usbAddress());
1048  d->tx_log = fopen(fname,"wb");
1049  }
1050  fwrite(data,1,*written,d->tx_log);
1051 #ifdef LOG_RX_TX_FLUSH
1052  fflush(d->tx_log);
1053 #endif
1054  }
1055 #endif
1056 
1057  return (d->m_lastResult = XRV_OK);
1058 }
1059 
1060 /*! \brief The USB bus number this device is on (libusb/linux only) */
1062 {
1063 #ifdef USE_WINUSB
1064  return 0;
1065 #else
1066  if (!d->m_deviceHandle)
1067  return 0;
1068 
1071 #endif
1072 }
1073 
1074 /*! \brief The address of the device (libusb/linux only) */
1076 {
1077 #ifdef USE_WINUSB
1078  return 0;
1079 #else
1080  if (!d->m_deviceHandle)
1081  return 0;
1082 
1085 #endif
1086 }
1087 
1088 //! Retrieve the port name that was last successfully opened.
1090 {
1091  portname = d->m_portname;
1092 }
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
libUSB_attach_kernel_driver attach_kernel_driver
Definition: xslibusb.h:50
Operation was performed successfully.
Definition: xsens_std.h:32
#define xsStartThread(func, param, pid)
Start a function as a thread.
Definition: xsthread.h:86
XSTYPES_DLL_API int XsPortInfo_usbAddress(XsPortInfo const *thisPtr)
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
#define XS_MAX_FILENAME_LENGTH
Definition: iointerface.h:30
uint8_t usbAddress() const
The address of the device (libusb/linux only)
unsigned __int16 uint16_t
Definition: rptypes.h:46
libUSB_close close
Definition: xslibusb.h:48
A timeout occurred.
Definition: xsens_std.h:60
UsbInterface()
Default constructor, initializes all members to their default values.
libUSB_unref_device unref_device
Definition: xslibusb.h:53
uint32_t getTimeout(void) const
Return the current timeout value.
UsbInterfacePrivate * d
Definition: usbinterface.h:70
bool isOpen(void) const
Return whether the USB communication port is open or not.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
libUSB_open open
Definition: xslibusb.h:47
GLenum GLint ref
Definition: glext.h:3888
Operation not implemented in this version (yet)
Definition: xsens_std.h:59
HANDLE XsIoHandle
Definition: xsfilepos.h:37
libUSB_init init
Definition: xslibusb.h:45
struct XsByteArray XsByteArray
Definition: xsbytearray.h:25
XsResultValue waitForData(XsSize maxLength, void *data, XsSize *length=NULL)
Wait for data to arrive or a timeout to occur.
The specified i/o device can not be opened.
Definition: xsens_std.h:69
libUSB_free_config_descriptor free_config_descriptor
Definition: xslibusb.h:57
UsbContext()
Create the USB context.
const char * libusbErrorToString(int libusbError)
Convert a libusb error to a human-readable string.
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:17
int BOOL
Definition: xstypedefs.h:62
No flag set.
Definition: xstypedefs.h:38
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
A required port could not be found.
Definition: xsens_std.h:94
Operation is invalid at this point.
Definition: xsens_std.h:67
uint32_t getTimeOfDay(tm *date_, time_t *secs_)
A platform-independent clock.
Definition: xsens_time.cpp:29
XsResultValue libusbErrorToXrv(int libusbError, XsResultValue hint=XRV_ERROR)
Map a libusb_error to XsResultValue.
No internal memory available.
Definition: xsens_std.h:63
unsigned char uint8_t
Definition: rptypes.h:43
libUSB_free_device_list free_device_list
Definition: xslibusb.h:63
void setRawIo(bool enable)
Sets the RAWIO mode of the USB interface.
XsResultValue m_lastResult
The last result of an operation.
libusb_device_handle * m_deviceHandle
XsResultValue
Xsens result values.
Definition: xsresultvalue.h:26
void msleep(uint32_t ms)
A platform-independent sleep routine.
Definition: xsens_time.cpp:78
__int64 int64_t
Definition: rptypes.h:51
uint32_t m_endTime
The time at which an operation will end in ms, used by several functions.
#define FALSE
Definition: jmorecfg.h:227
No serial port opened for reading/writing.
Definition: xsens_std.h:92
libUSB_get_device_list get_device_list
Definition: xslibusb.h:62
Class for dynamic loading of winusb.
bool getRawIo(void)
Retrieves the state of the RAWIO mode of the USB interface.
libUSB_claim_interface claim_interface
Definition: xslibusb.h:54
uint8_t usbBus() const
The USB bus number this device is on (libusb/linux only)
The requested item was not found.
Definition: xsens_std.h:64
libUSB_get_device_descriptor get_device_descriptor
Definition: xslibusb.h:61
A context manager for libusb.
XsResultValue setTimeout(uint32_t ms)
Set the default timeout value to use in blocking operations.
XsResultValue closeUsb(void)
Close the USB communication port.
#define TRUE
Definition: jmorecfg.h:230
libUSB_get_device get_device
Definition: xslibusb.h:59
virtual XsResultValue writeData(const XsByteArray &data, XsSize *written=NULL)
Write the data to the USB port.
HANDLE XsThread
A handle for a thread.
Definition: xsthread.h:78
#define xsGetCurrentThreadHandle()
Definition: xsthread.h:91
libUSB_kernel_driver_active kernel_driver_active
Definition: xslibusb.h:49
~UsbInterface()
Destructor, de-initializes, frees memory allocated for buffers, etc.
libUSB_exit exit
Definition: xslibusb.h:46
Private object for UsbInterface.
#define JLDEBUG(...)
XsResultValue getLastResult(void) const
Return the error code of the last operation.
libUSB_detach_kernel_driver detach_kernel_driver
Definition: xslibusb.h:51
DWORD XsThreadId
Definition: xsthread.h:82
#define xsSetThreadPriority(thrd, prio)
Definition: xsthread.h:94
void getPortName(XsString &portname) const
Retrieve the port name that was last successfully opened.
static UsbContext & getContextManager()
GLuint GLsizei GLsizei * length
Definition: glext.h:3900
An invalid parameter is supplied.
Definition: xsens_std.h:53
XSTYPES_DLL_API int64_t XsTime_timeStampNow(XsTimeStamp *now)
FILE XsFileHandle
Definition: xsfilepos.h:50
GLuint address
Definition: glext.h:6078
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
#define XSENS_INVALID_THREAD
Definition: xsthread.h:67
XsResultValue flushData(void)
Flush all data in the buffers to and from the device.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
A generic error occurred.
Definition: xsens_std.h:58
libUSB_get_device_address get_device_address
Definition: xslibusb.h:60
GLsizei maxLength
Definition: glext.h:4504
#define JLTRACE(...)
GLuint res
Definition: glext.h:6298
XsResultValue close(void)
Close the USB communication port.
struct XsString XsString
Definition: xsstring.h:34
virtual XsResultValue readData(XsSize maxLength, XsByteArray &data)
Read data from the USB port and put it into the data buffer.
unsigned __int32 uint32_t
Definition: rptypes.h:49
libUSB_release_interface release_interface
Definition: xslibusb.h:55
libUSB_get_bus_number get_bus_number
Definition: xslibusb.h:58
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
libUSB_get_active_config_descriptor get_active_config_descriptor
Definition: xslibusb.h:56
#define JLALERT(...)
XsResultValue open(const XsPortInfo &portInfo, uint32_t readBufSize=0, uint32_t writeBufSize=0)
Open a communication channel to the given USB port name.
Unexpected message received (e.g. no acknowledge message received)
Definition: xsens_std.h:65
An I/O device is already opened with this object.
Definition: xsens_std.h:71
XSTYPES_DLL_API void xsNameThisThread(const char *threadName)
XSTYPES_DLL_API int XsPortInfo_usbBus(XsPortInfo const *thisPtr)
libUSB_bulk_transfer bulk_transfer
Definition: xslibusb.h:65
~UsbContext()
Destroy the USB context.
libUSB_ref_device ref_device
Definition: xslibusb.h:52



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019