MRPT  1.9.9
cmtscan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*! \file cmtscan.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 
37 #include <devguid.h>
38 #include <regstr.h>
39 #include <setupapi.h>
40 #include <string.h>
41 #else
42 #include <dirent.h>
43 #include <cstdlib>
44 #include <cstring>
45 #endif
46 
47 #include "cmt3.h"
48 #include "cmtscan.h"
49 #include "xsens_janitors.h"
50 
51 namespace xsens
52 {
53 #ifdef _LOG_CMT_SCAN
54 #define SCANLOG CMTLOG
55 #else
56 #define SCANLOG(...)
57 #endif
58 
59 bool abortScan = false;
60 
62  CmtPortInfo& portInfo, uint32_t baud, uint32_t singleScanTimeout,
63  uint32_t scanTries)
64 {
65  uint32_t baudrate;
66  Cmt3 port;
67  port.setGotoConfigTries(scanTries ? (uint16_t)scanTries : 1);
68  port.setTimeoutConfig(singleScanTimeout);
70 
71  if (baud == 0)
72  baudrate = CMT_BAUD_RATE_115K2;
73  else
74  baudrate = baud;
75 
76  while (!abortScan)
77  {
78 // try to connect at current baudrate
79 #ifdef _WIN32
80  if ((res = port.openPort(portInfo.m_portNr, baudrate)) == XRV_OK)
81 #else
82  if ((res = port.openPort(portInfo.m_portName, baudrate)) == XRV_OK)
83 #endif
84  {
85  SCANLOG("SP: L3 port-check returns OK\n");
86  portInfo.m_baudrate = baudrate;
87  portInfo.m_deviceId = port.getMasterId();
88  return true; // this also closes the port
89  }
90  // failed, determine if we need to scan other baudrates or not
91  if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA &&
93  {
94  SCANLOG("SP: L3 port-check returned ERROR, aborting\n");
95  return false;
96  }
97 
98  SCANLOG(
99  "SP: L3 port-check returned TIMEOUT, check next baudrate or "
100  "abort\n");
101  // not detected, try next baudrate
102  if (baud != 0) return false;
103  switch (baudrate)
104  {
105  default:
106  case CMT_BAUD_RATE_115K2:
107  baudrate = CMT_BAUD_RATE_460K8;
108  break;
109  case CMT_BAUD_RATE_460K8:
110  baudrate = CMT_BAUD_RATE_921K6;
111  break;
112  case CMT_BAUD_RATE_921K6:
113  baudrate = CMT_BAUD_RATE_230K4;
114  break;
115  case CMT_BAUD_RATE_230K4:
116  baudrate = CMT_BAUD_RATE_57K6;
117  break;
118  case CMT_BAUD_RATE_57K6:
119  baudrate = CMT_BAUD_RATE_38K4;
120  break;
121  case CMT_BAUD_RATE_38K4:
122  baudrate = CMT_BAUD_RATE_19K2;
123  break;
124  case CMT_BAUD_RATE_19K2:
125  baudrate = CMT_BAUD_RATE_9600;
126  break;
127  case CMT_BAUD_RATE_9600:
128  return false; // could not detect Xsens sensor, return false
129  }
130  }
131  return false;
132 }
133 
135  List<CmtPortInfo>& ports, uint32_t baudrate, uint32_t singleScanTimeout,
136  uint32_t scanTries)
137 {
138  CmtPortInfo current;
139  ports.clear(); // clear the list
140 #ifdef _WIN32
141  HDEVINFO hDevInfo;
142  SP_DEVINFO_DATA DeviceInfoData;
143  DWORD i;
144 
145  // Create a HDEVINFO with all present devices.
146 
147  // GUID for Ports: 4D36E978-E325-11CE-BFC1-08002BE10318
148  GUID portGuid = {0x4D36E978,
149  0xE325,
150  0x11CE,
151  {0xBF, 0xC1, 0x08, 0x00, 0x2B, 0xE1, 0x03, 0x18}};
152 
153  // "4D36E978-E325-11CE-BFC1-08002BE10318"
154  hDevInfo =
155  SetupDiGetClassDevs(&portGuid, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE);
156 
157  if (hDevInfo == INVALID_HANDLE_VALUE) return false;
158 
159  // Enumerate through all devices in Set.
160  DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
161  for (i = 0;
162  !abortScan && SetupDiEnumDeviceInfo(hDevInfo, i, &DeviceInfoData); ++i)
163  {
164  DWORD DataT;
165  char buffer[256];
166  bool isBT = false;
167 
168 //
169 // Call function with null to begin with,
170 // then use the returned buffer size
171 // to Alloc the buffer. Keep calling until
172 // success or an unknown failure.
173 //
174 #if 1
175  if (SetupDiGetDeviceRegistryProperty(
176  hDevInfo, &DeviceInfoData, SPDRP_MFG, &DataT, (PBYTE)buffer,
177  256, nullptr))
178  {
179  // on failure, this is not an Xsens Device
180  // on success, we need to check if the device is an Xsens Device
181  // if (_strnicmp(buffer,"xsens",5))
182  // scan = true;
183  // else
184  if (!_strnicmp(
185  buffer, "(Standard port types)",
186  strlen("(Standard port types)")))
187  continue;
188  if (_strnicmp(buffer, "xsens", 5)) // if this is NOT an xsens
189  // device, treat it as a BT
190  // device
191  {
192  isBT = true;
193  if (_strnicmp(buffer, "WIDCOMM", 7)) // if this is NOT a
194  // WIDCOMM (Ezureo / TDK
195  // stack), skip it
196  continue;
197  }
198  }
199 #endif
200  // we found an Xsens Device, add its port nr to the list
201  // Get the registry key which stores the ports settings
202  HKEY hDeviceKey = SetupDiOpenDevRegKey(
203  hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV,
204  KEY_QUERY_VALUE);
205  if (hDeviceKey != INVALID_HANDLE_VALUE)
206  {
207  // Read in the name of the port
208  char pszPortName[256];
209  DWORD dwSize = 256;
210  DWORD dwType = 0;
211  if ((RegQueryValueExA(
212  hDeviceKey, "PortName", nullptr, &dwType,
213  (LPBYTE)pszPortName, &dwSize) == ERROR_SUCCESS) &&
214  (dwType == REG_SZ))
215  {
216  // If it looks like "COMX" then
217  // add it to the array which will be returned
218  int32_t nLen = (int32_t)strlen(pszPortName);
219  if (nLen > 3)
220  {
221  if (_strnicmp(pszPortName, "COM", 3)) continue;
222  int32_t nPort = atoi(&pszPortName[3]);
223  if (nPort)
224  {
225  current.m_portNr = (uint8_t)nPort;
226  if (isBT)
228  else
229  current.m_baudrate = baudrate;
230  ports.append(current);
231  }
232  }
233  }
234  }
235  // Close the key now that we are finished with it
236  RegCloseKey(hDeviceKey);
237  }
238 
239  // Cleanup
240 
241  SetupDiDestroyDeviceInfoList(hDevInfo);
242 
243  // Now sort the list by ascending port nr
244  ports.sortAscending();
245 
246  // Add the standard com ports 1 and 2 unless they are already in the list
247  bool add1 = true, add2 = true;
248  if ((ports.length() > 0) && (ports[0].m_portNr == 1)) add1 = false;
249  if (ports.length() > 0)
250  {
251  if (ports[0].m_portNr == 2)
252  add2 = false;
253  else if (ports.length() > 1)
254  if (ports[1].m_portNr == 2) add2 = false;
255  }
256  if (add1)
257  {
258  current.m_portNr = 1;
259  current.m_baudrate = baudrate;
260  ports.append(current);
261  }
262  if (add2)
263  {
264  current.m_portNr = 2;
265  current.m_baudrate = baudrate;
266  ports.append(current);
267  }
268 #else
269  DIR* dir;
270  struct dirent* entry;
271 
272  if ((dir = opendir("/dev/")) == nullptr) return false;
273 
274  while ((entry = readdir(dir)))
275  if (strncmp("ttyS", entry->d_name, 4) == 0 ||
276  strncmp("ttyUSB", entry->d_name, 6) == 0)
277  {
278  sprintf(current.m_portName, "/dev/%s", entry->d_name);
279  current.m_baudrate = baudrate;
280  ports.append(current);
281  }
282  closedir(dir);
283 
284  ports.sortAscending();
285 #endif
286 
287  // try to connect so we can detect if there really is an MT / XM attached
288  unsigned p = 0;
289  while (!abortScan && p < ports.length())
290  {
291  if (cmtScanPort(
292  ports[p], ports[p].m_baudrate, singleScanTimeout, scanTries))
293  ++p;
294  else
295  ports.remove(p);
296  }
297 
298  if (abortScan) return abortScan = false;
299 
300  // Now sort the final list by ascending port nr
301  ports.sortAscending();
302  abortScan = false;
303  return true;
304 }
305 
306 } // namespace xsens
High-level communication class.
Definition: cmt3.h:39
Operation was performed successfully.
Definition: xsens_std.h:35
CmtDeviceId getMasterId(void)
Definition: cmt3.cpp:1002
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
unsigned __int16 uint16_t
Definition: rptypes.h:47
XsensResultValue openPort(const char *portName, const uint32_t baudRate=CMT_DEFAULT_BAUD_RATE)
Definition: cmt3.cpp:1566
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:59
#define CMT_BAUD_RATE_230K4
Definition: cmtdef.h:751
GLuint buffer
Definition: glext.h:3928
Structure for storing information about a serial port.
Definition: cmtdef.h:1168
uint32_t m_deviceId
The device Id of the detected Xsens device.
Definition: cmtdef.h:1173
int _strnicmp(const char *str, const char *subStr, size_t count) noexcept
An OS-independent version of strnicmp.
Definition: os.cpp:346
#define CMT_BAUD_RATE_9600
Definition: cmtdef.h:744
#define CMT_BAUD_RATE_57K6
Definition: cmtdef.h:749
char m_portName[270]
The port name.
Definition: cmtdef.h:1177
#define SCANLOG(...)
Definition: cmtscan.cpp:56
unsigned char uint8_t
Definition: rptypes.h:44
XsensResultValue
Xsens return values.
Definition: xsens_std.h:31
uint32_t m_baudrate
The baudrate at which an Xsens device was detected.
Definition: cmtdef.h:1171
#define CMT_BAUD_RATE_38K4
Definition: cmtdef.h:748
#define CMT_BAUD_RATE_460K8
Definition: cmtdef.h:752
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:61
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:64
uint8_t m_portNr
The port number.
Definition: cmtdef.h:1175
__int32 int32_t
Definition: rptypes.h:49
auto dir
XsensResultValue setTimeoutConfig(const uint32_t timeout=CMT3_DEFAULT_TIMEOUT_CONF)
Set the configuration mode timeout.
Definition: cmt3.cpp:2556
uint32_t length(void) const
Returns the number of items currently in the list.
Definition: xsens_list.h:159
#define CMT_BAUD_RATE_115K2
Definition: cmtdef.h:750
The namespace of all Xsens software since 2006.
Definition: cmt1.cpp:95
bool cmtScanPorts(List< CmtPortInfo > &ports, uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
Scan COM ports for connected Xsens devices.
Definition: cmtscan.cpp:134
#define CMT_BAUD_RATE_921K6
Definition: cmtdef.h:753
GLuint res
Definition: glext.h:7385
#define CMT_BAUD_RATE_19K2
Definition: cmtdef.h:746
unsigned __int32 uint32_t
Definition: rptypes.h:50
GLfloat GLfloat p
Definition: glext.h:6398
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:191
XsensResultValue setGotoConfigTries(const uint16_t tries)
Set the number of times the gotoConfig function will attempt a gotoConfig before failing.
Definition: cmt3.cpp:2179



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 928d0abbd Sun Oct 13 02:28:49 2019 +0200 at dom oct 13 02:30:11 CEST 2019