Main MRPT website > C++ reference for MRPT 1.5.6
cmtscan.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 
10 /*! \file ScanPorts.cpp
11 
12  For information about objects in this file, see the appropriate header:
13  \ref ScanPorts.h
14 
15  \section FileCopyright Copyright Notice
16  Copyright (C) Xsens Technologies B.V., 2006. All rights reserved.
17 
18  This source code is intended for use only by Xsens Technologies BV and
19  those that have explicit written permission to use it from
20  Xsens Technologies BV.
21 
22  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
23  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
24  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
25  PARTICULAR PURPOSE.
26 
27  \section FileChangelog Changelog
28  \par 2006-06-08, v0.0.1
29  \li Job Mulder: Created
30  \par 2006-07-21, v0.1.0
31  \li Job Mulder: Updated file for release 0.1.0
32 */
33 
34 #ifdef _WIN32
35 # include <windows.h>
36 # include <string.h>
37 # include <setupapi.h>
38 # include <devguid.h>
39 # include <regstr.h>
40 #else
41 # include <stdlib.h>
42 # include <string.h>
43 # include <dirent.h>
44 #endif
45 
46 #include "cmt3.h"
47 #include "cmtscan.h"
48 #include "xsens_janitors.h"
49 
50 namespace xsens {
51 
52 #ifdef _LOG_CMT_SCAN
53 # define SCANLOG CMTLOG
54 #else
55 # define SCANLOG(...)
56 #endif
57 
58 bool abortScan = false;
59 
60 bool cmtScanPort(CmtPortInfo& portInfo, uint32_t baud, uint32_t singleScanTimeout, uint32_t scanTries)
61 {
62  uint32_t baudrate;
63  Cmt3 port;
64  port.setGotoConfigTries(scanTries?(uint16_t)scanTries:1);
65  port.setTimeoutConfig(singleScanTimeout);
67 
68  if (baud == 0)
69  baudrate = CMT_BAUD_RATE_115K2;
70  else
71  baudrate = baud;
72 
73  while(!abortScan)
74  {
75  // try to connect at current baudrate
76 #ifdef _WIN32
77  if ((res = port.openPort(portInfo.m_portNr,baudrate)) == XRV_OK)
78 #else
79  if ((res = port.openPort(portInfo.m_portName,baudrate)) == XRV_OK)
80 #endif
81  {
82  SCANLOG("SP: L3 port-check returns OK\n");
83  portInfo.m_baudrate = baudrate;
84  portInfo.m_deviceId = port.getMasterId();
85  return true; // this also closes the port
86  }
87  // failed, determine if we need to scan other baudrates or not
88  if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL)
89  {
90  SCANLOG("SP: L3 port-check returned ERROR, aborting\n");
91  return false;
92  }
93 
94  SCANLOG("SP: L3 port-check returned TIMEOUT, check next baudrate or abort\n");
95  // not detected, try next baudrate
96  if (baud != 0)
97  return false;
98  switch(baudrate)
99  {
100  default:
101  case CMT_BAUD_RATE_115K2:
102  baudrate = CMT_BAUD_RATE_460K8; break;
103  case CMT_BAUD_RATE_460K8:
104  baudrate = CMT_BAUD_RATE_921K6; break;
105  case CMT_BAUD_RATE_921K6:
106  baudrate = CMT_BAUD_RATE_230K4; break;
107  case CMT_BAUD_RATE_230K4:
108  baudrate = CMT_BAUD_RATE_57K6; break;
109  case CMT_BAUD_RATE_57K6:
110  baudrate = CMT_BAUD_RATE_38K4; break;
111  case CMT_BAUD_RATE_38K4:
112  baudrate = CMT_BAUD_RATE_19K2; break;
113  case CMT_BAUD_RATE_19K2:
114  baudrate = CMT_BAUD_RATE_9600; break;
115  case CMT_BAUD_RATE_9600:
116  return false; // could not detect Xsens sensor, return false
117  }
118  }
119  return false;
120 }
121 
122 bool cmtScanPorts(List<CmtPortInfo>& ports,uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
123 {
124  CmtPortInfo current;
125  ports.clear(); // clear the list
126 #ifdef _WIN32
127  HDEVINFO hDevInfo;
128  SP_DEVINFO_DATA DeviceInfoData;
129  DWORD i;
130 
131  // Create a HDEVINFO with all present devices.
132 
133  // GUID for Ports: 4D36E978-E325-11CE-BFC1-08002BE10318
134  GUID portGuid =
135  {0x4D36E978,0xE325,0x11CE,{0xBF,0xC1,0x08,0x00,0x2B,0xE1,0x03,0x18}};
136 
137  // "4D36E978-E325-11CE-BFC1-08002BE10318"
138  hDevInfo = SetupDiGetClassDevs(&portGuid, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE);
139 
140  if (hDevInfo == INVALID_HANDLE_VALUE)
141  return false;
142 
143  // Enumerate through all devices in Set.
144  DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
145  for (i=0;!abortScan && SetupDiEnumDeviceInfo(hDevInfo,i,&DeviceInfoData);++i)
146  {
147  DWORD DataT;
148  char buffer[256];
149  bool isBT = false;
150 
151  //
152  // Call function with null to begin with,
153  // then use the returned buffer size
154  // to Alloc the buffer. Keep calling until
155  // success or an unknown failure.
156  //
157 #if 1
158  if (SetupDiGetDeviceRegistryProperty(hDevInfo,
159  &DeviceInfoData,
160  SPDRP_MFG,
161  &DataT,
162  (PBYTE)buffer,
163  256,
164  NULL))
165  {
166  // on failure, this is not an Xsens Device
167  // on success, we need to check if the device is an Xsens Device
168  //if (_strnicmp(buffer,"xsens",5))
169  // scan = true;
170  //else
171  if (!_strnicmp(buffer,"(Standard port types)",strlen("(Standard port types)")))
172  continue;
173  if (_strnicmp(buffer,"xsens",5)) // if this is NOT an xsens device, treat it as a BT device
174  {
175  isBT = true;
176  if (_strnicmp(buffer,"WIDCOMM",7)) // if this is NOT a WIDCOMM (Ezureo / TDK stack), skip it
177  continue;
178  }
179  }
180 #endif
181  // we found an Xsens Device, add its port nr to the list
182  //Get the registry key which stores the ports settings
183  HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
184  if (hDeviceKey != INVALID_HANDLE_VALUE)
185  {
186  //Read in the name of the port
187  char pszPortName[256];
188  DWORD dwSize = 256;
189  DWORD dwType = 0;
190  if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ))
191  {
192  //If it looks like "COMX" then
193  //add it to the array which will be returned
194  int32_t nLen = (int32_t) strlen(pszPortName);
195  if (nLen > 3)
196  {
197  if (_strnicmp(pszPortName, "COM", 3))
198  continue;
199  int32_t nPort = atoi(&pszPortName[3]);
200  if (nPort)
201  {
202  current.m_portNr = (uint8_t) nPort;
203  if (isBT)
205  else
206  current.m_baudrate = baudrate;
207  ports.append(current);
208  }
209  }
210  }
211  }
212  //Close the key now that we are finished with it
213  RegCloseKey(hDeviceKey);
214  }
215 
216  // Cleanup
217 
218  SetupDiDestroyDeviceInfoList(hDevInfo);
219 
220  // Now sort the list by ascending port nr
221  ports.sortAscending();
222 
223  // Add the standard com ports 1 and 2 unless they are already in the list
224  bool add1 = true,
225  add2 = true;
226  if ((ports.length() > 0) && (ports[0].m_portNr == 1))
227  add1 = false;
228  if (ports.length() > 0)
229  {
230  if (ports[0].m_portNr == 2)
231  add2 = false;
232  else
233  if (ports.length() > 1)
234  if (ports[1].m_portNr == 2)
235  add2 = false;
236  }
237  if (add1)
238  {
239  current.m_portNr = 1;
240  current.m_baudrate = baudrate;
241  ports.append(current);
242  }
243  if (add2)
244  {
245  current.m_portNr = 2;
246  current.m_baudrate = baudrate;
247  ports.append(current);
248  }
249 #else
250  DIR *dir;
251  struct dirent *entry;
252 
253  if ((dir = opendir("/dev/")) == NULL)
254  return false;
255 
256  while ((entry = readdir(dir)))
257  if (strncmp("ttyS", entry->d_name, 4) == 0 || strncmp("ttyUSB", entry->d_name, 6) == 0)
258  {
259  sprintf(current.m_portName, "/dev/%s", entry->d_name);
260  current.m_baudrate = baudrate;
261  ports.append(current);
262  }
263  closedir(dir);
264 
265  ports.sortAscending();
266 #endif
267 
268  // try to connect so we can detect if there really is an MT / XM attached
269  unsigned p = 0;
270  while (!abortScan && p < ports.length())
271  {
272  if (cmtScanPort(ports[p],ports[p].m_baudrate,singleScanTimeout,scanTries))
273  ++p;
274  else
275  ports.remove(p);
276  }
277 
278  if (abortScan)
279  return abortScan = false;
280 
281  // Now sort the final list by ascending port nr
282  ports.sortAscending();
283  abortScan = false;
284  return true;
285 }
286 
287 } // end of xsens namespace
High-level communication class.
Definition: cmt3.h:36
DWORD dwSize
Definition: wglew.h:682
Operation was performed successfully.
Definition: xsens_std.h:32
CmtDeviceId getMasterId(void)
Definition: cmt3.cpp:972
void clear(void)
Clears the list without explicitly deleting anything.
Definition: xsens_list.hpp:126
void append(const T &item)
Adds an item to the end of the list.
Definition: xsens_list.hpp:150
uint32_t length(void) const
Returns the number of items currently in the list.
Definition: xsens_list.h:143
unsigned __int16 uint16_t
Definition: rptypes.h:46
XsensResultValue openPort(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE)
Definition: cmt3.cpp:1541
A timeout occurred.
Definition: xsens_std.h:60
void remove(const uint32_t index) XSENS_LIST_THROW
Removes an item at the given index in the list.
Definition: xsens_list.hpp:775
bool abortScan
Set to true from another thread to abort any scan currently in progress.
Definition: cmtscan.cpp:58
Operation aborted because of no data read.
Definition: xsens_std.h:61
#define CMT_BAUD_RATE_230K4
Definition: cmtdef.h:751
Structure for storing information about a serial port.
Definition: cmtdef.h:1097
uint32_t m_deviceId
The device Id of the detected Xsens device.
Definition: cmtdef.h:1099
#define CMT_BAUD_RATE_9600
Definition: cmtdef.h:744
#define CMT_BAUD_RATE_57K6
Definition: cmtdef.h:749
#define SCANLOG(...)
Definition: cmtscan.cpp:55
unsigned char uint8_t
Definition: rptypes.h:43
XsensResultValue
Xsens return values.
Definition: xsens_std.h:30
GLuint buffer
Definition: glew.h:1585
uint32_t m_baudrate
The baudrate at which an Xsens device was detected.
Definition: cmtdef.h:1098
#define CMT_BAUD_RATE_38K4
Definition: cmtdef.h:748
#define CMT_BAUD_RATE_460K8
Definition: cmtdef.h:752
GLfloat GLfloat p
Definition: glew.h:10113
The in-config check of the device failed.
Definition: xsens_std.h:98
bool cmtScanPort(CmtPortInfo &portInfo, uint32_t baud, uint32_t singleScanTimeout, uint32_t scanTries)
Scan a single COM port for connected Xsens devices.
Definition: cmtscan.cpp:60
GLuint res
Definition: glew.h:7143
void sortAscending(void)
Sorts the list in an ascending order, using the T::< operator.
Definition: xsens_list.hpp:453
Dynamic list class.
Definition: xsens_list.h:60
uint8_t m_portNr
The port number.
Definition: cmtdef.h:1100
__int32 int32_t
Definition: rptypes.h:48
XsensResultValue setTimeoutConfig(const uint32_t timeout=CMT3_DEFAULT_TIMEOUT_CONF)
Set the configuration mode timeout.
Definition: cmt3.cpp:2490
#define CMT_BAUD_RATE_115K2
Definition: cmtdef.h:750
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
bool cmtScanPorts(List< CmtPortInfo > &ports, uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
Scan COM ports for connected Xsens devices.
Definition: cmtscan.cpp:122
#define CMT_BAUD_RATE_921K6
Definition: cmtdef.h:753
char m_portName[32]
The port name.
Definition: cmtdef.h:1101
#define CMT_BAUD_RATE_19K2
Definition: cmtdef.h:746
unsigned __int32 uint32_t
Definition: rptypes.h:49
int BASE_IMPEXP _strnicmp(const char *str, const char *subStr, size_t count) MRPT_NO_THROWS
An OS-independent version of strnicmp.
Definition: os.cpp:344
XsensResultValue setGotoConfigTries(const uint16_t tries)
Set the number of times the gotoConfig function will attempt a gotoConfig before failing.
Definition: cmt3.cpp:2107



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018