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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020