cmtscan.cpp

00001 /*! \file ScanPorts.cpp
00002 
00003         For information about objects in this file, see the appropriate header:
00004         \ref ScanPorts.h
00005 
00006         \section FileCopyright Copyright Notice 
00007         Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.
00008         
00009         This source code is intended for use only by Xsens Technologies BV and
00010         those that have explicit written permission to use it from
00011         Xsens Technologies BV.
00012         
00013         THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
00014         KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
00015         IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
00016         PARTICULAR PURPOSE.
00017         
00018         \section FileChangelog  Changelog
00019         \par 2006-06-08, v0.0.1
00020         \li Job Mulder: Created
00021         \par 2006-07-21, v0.1.0
00022         \li Job Mulder: Updated file for release 0.1.0
00023 */
00024 
00025 #ifdef _WIN32
00026 #       include <windows.h>
00027 #       include <string.h>
00028 #       include <setupapi.h>
00029 #       include <devguid.h>
00030 #       include <regstr.h>
00031 #else
00032 #       include <stdlib.h>
00033 #       include <string.h>
00034 #       include <dirent.h>
00035 #endif
00036 
00037 #include "cmt3.h"
00038 #include "cmtscan.h"
00039 #include "xsens_janitors.h"
00040 
00041 namespace xsens {
00042 
00043 #ifdef _LOG_CMT_SCAN
00044 #       define SCANLOG          CMTLOG
00045 #else
00046 #       define SCANLOG(...)
00047 #endif
00048 
00049 bool abortScan = false;
00050 
00051 bool cmtScanPort(CmtPortInfo& portInfo, uint32_t baud, uint32_t singleScanTimeout, uint32_t scanTries)
00052 {
00053         uint32_t baudrate;
00054         Cmt3 port;
00055         port.setGotoConfigTries(scanTries?(uint16_t)scanTries:1);
00056         port.setTimeoutConfig(singleScanTimeout);
00057         XsensResultValue res;
00058 
00059         if (baud == 0)
00060                 baudrate = CMT_BAUD_RATE_115K2;
00061         else
00062                 baudrate = baud;
00063 
00064         CmtPortInfo theport = portInfo;
00065         std::cout << theport.m_baudrate << "|" << theport.m_deviceId << "|" << theport.m_portName << "|" << theport.m_portNr << std::endl;
00066 
00067         while(!abortScan)
00068         {
00069                 // try to connect at current baudrate
00070 #ifdef _WIN32
00071                 if ((res = port.openPort(portInfo.m_portNr,baudrate)) == XRV_OK)
00072 #else
00073                 if ((res = port.openPort(portInfo.m_portName,baudrate)) == XRV_OK)
00074 #endif
00075                 {
00076                         std::cout << "success" << std::endl;
00077                         SCANLOG("SP: L3 port-check returns OK\n");
00078                         portInfo.m_baudrate = baudrate;
00079                         portInfo.m_deviceId = port.getMasterId();
00080                         return true;    // this also closes the port
00081                 }
00082                 // failed, determine if we need to scan other baudrates or not
00083                 if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL)
00084                 {
00085                         std::cout << "failed" << std::endl;
00086                         SCANLOG("SP: L3 port-check returned ERROR, aborting\n");
00087                         return false;
00088                 }
00089 
00090                 std::cout << "timeout" << std::endl;
00091 
00092                 SCANLOG("SP: L3 port-check returned TIMEOUT, check next baudrate or abort\n");
00093                 // not detected, try next baudrate
00094                 if (baud != 0)
00095                         return false;
00096                 switch(baudrate)
00097                 {
00098                 default:
00099                 case CMT_BAUD_RATE_115K2:
00100                         baudrate = CMT_BAUD_RATE_921K6; break;
00101                 case CMT_BAUD_RATE_921K6:
00102                         baudrate = CMT_BAUD_RATE_460K8; break;
00103                 case CMT_BAUD_RATE_460K8:
00104                         baudrate = CMT_BAUD_RATE_230K4; break;
00105                 case CMT_BAUD_RATE_230K4:
00106                         baudrate = CMT_BAUD_RATE_57K6; break;
00107                 case CMT_BAUD_RATE_57K6:
00108                         baudrate = CMT_BAUD_RATE_38K4; break;
00109                 case CMT_BAUD_RATE_38K4:
00110                         baudrate = CMT_BAUD_RATE_19K2; break;
00111                 case CMT_BAUD_RATE_19K2:
00112                         baudrate = CMT_BAUD_RATE_9600; break;
00113                 case CMT_BAUD_RATE_9600:
00114                         return false;   // could not detect Xsens sensor, return false
00115                 }
00116         }
00117         return false;
00118 }
00119 
00120 bool cmtScanForIMU(CmtPortInfo & port, uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
00121 {
00122         std::cout << "initial "<< port.m_baudrate << "|" << port.m_deviceId << "|" << port.m_portName << "|" << port.m_portNr << std::endl;
00123         bool theResult = cmtScanPort(port,port.m_baudrate,singleScanTimeout,scanTries);
00124         std::cout << "final "<< port.m_baudrate << "|" << port.m_deviceId << "|" << port.m_portName << "|" << port.m_portNr << std::endl;
00125         if (theResult)
00126         {
00127                 std::cout << "success" << std::endl;
00128                 return true;
00129         }
00130         else
00131         {
00132                 std::cout << "failed" << std::endl;
00133         }
00134         return false;
00135 }
00136 
00137 bool cmtScanPorts(List<CmtPortInfo>& ports,uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries)
00138 {
00139         CmtPortInfo current = {0,0,0,""};
00140         ports.clear();  // clear the list
00141 #ifdef _WIN32
00142         HDEVINFO hDevInfo;
00143         SP_DEVINFO_DATA DeviceInfoData;
00144         DWORD i;
00145 
00146         // Create a HDEVINFO with all present devices.
00147 
00148         // GUID for Ports: 4D36E978-E325-11CE-BFC1-08002BE10318
00149         GUID portGuid = 
00150                 {0x4D36E978,0xE325,0x11CE,{0xBF,0xC1,0x08,0x00,0x2B,0xE1,0x03,0x18}};
00151                 
00152         //      "4D36E978-E325-11CE-BFC1-08002BE10318"
00153         hDevInfo = SetupDiGetClassDevs(&portGuid, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE);
00154 
00155         if (hDevInfo == INVALID_HANDLE_VALUE)
00156                 return false;
00157 
00158         // Enumerate through all devices in Set.
00159         DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
00160         for (i=0;!abortScan && SetupDiEnumDeviceInfo(hDevInfo,i,&DeviceInfoData);++i)
00161         {
00162                 DWORD DataT;
00163                 char buffer[256];
00164                 bool isBT = false;
00165 
00166                 //
00167                 // Call function with null to begin with,
00168                 // then use the returned buffer size
00169                 // to Alloc the buffer. Keep calling until
00170                 // success or an unknown failure.
00171                 //
00172 #if 1
00173                 if (SetupDiGetDeviceRegistryProperty(hDevInfo,
00174                                                 &DeviceInfoData,
00175                                                 SPDRP_MFG,
00176                                                 &DataT,
00177                                                 (PBYTE)buffer,
00178                                                 256,
00179                                                 NULL))
00180                 {
00181                         // on failure, this is not an Xsens Device
00182                         // on success, we need to check if the device is an Xsens Device
00183                         //if (_strnicmp(buffer,"xsens",5))
00184                         //      scan = true;
00185                         //else
00186                         if (!_strnicmp(buffer,"(Standard port types)",20))
00187                                 continue;
00188                         if (_strnicmp(buffer,"xsens",5))        // if this is NOT an xsens device, treat it as a BT device
00189                         {
00190                                 isBT = true;
00191                                 if (_strnicmp(buffer,"WIDCOMM",7))      // if this is NOT a WIDCOMM (Ezureo / TDK stack), skip it
00192                                         continue;
00193                         }
00194                 }
00195 #endif
00196                 // we found an Xsens Device, add its port nr to the list
00197                 //Get the registry key which stores the ports settings
00198                 HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
00199                 if (hDeviceKey != INVALID_HANDLE_VALUE)
00200                 {
00201                         //Read in the name of the port
00202                         char pszPortName[256];
00203                         DWORD dwSize = 256;
00204                         DWORD dwType = 0;
00205                         if ((RegQueryValueEx(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ))
00206                         {
00207                                 //If it looks like "COMX" then
00208                                 //add it to the array which will be returned
00209                                 int32_t nLen = (int32_t) strlen(pszPortName);
00210                                 if (nLen > 3)
00211                                 {
00212                                         if (_strnicmp(pszPortName, "COM", 3))
00213                                                 continue;
00214                                         int32_t nPort = atoi(&pszPortName[3]);
00215                                         if (nPort)
00216                                         {
00217                                                 current.m_portNr = (uint16_t) nPort;
00218                                                 if (isBT)
00219                                                         current.m_baudrate = CMT_BAUD_RATE_460K8;
00220                                                 else
00221                                                         current.m_baudrate = baudrate;
00222                                                 ports.append(current);
00223                                         }
00224                                 }
00225                         }
00226                 }
00227                 //Close the key now that we are finished with it
00228                 RegCloseKey(hDeviceKey);
00229         }
00230 
00231         //  Cleanup
00232 
00233         SetupDiDestroyDeviceInfoList(hDevInfo);
00234 
00235         // Now sort the list by ascending port nr
00236         ports.sortAscending();
00237 
00238         // Add the standard com ports 1 and 2 unless they are already in the list
00239         bool    add1 = true,
00240                         add2 = true;
00241         if ((ports.length() > 0) && (ports[0].m_portNr == 1))
00242                 add1 = false;
00243         if (ports.length() > 0)
00244         {
00245                 if (ports[0].m_portNr == 2)
00246                         add2 = false;
00247                 else
00248                         if (ports.length() > 1)
00249                                 if (ports[1].m_portNr == 2)
00250                                         add2 = false;
00251         }
00252         if (add1)
00253         {
00254                 current.m_portNr = 1;
00255                 current.m_baudrate = baudrate;
00256                 ports.append(current);
00257         }
00258         if (add2)
00259         {
00260                 current.m_portNr = 2;
00261                 current.m_baudrate = baudrate;
00262                 ports.append(current);
00263         }
00264 #else
00265         DIR *dir;
00266         struct dirent *entry;
00267         
00268         if ((dir = opendir("/dev/")) == NULL)
00269                 return false;
00270         
00271         while ((entry = readdir(dir)))
00272                 if ((strncmp("ttyS", entry->d_name, 4) == 0 || strncmp("ttyUSB", entry->d_name, 6) == 0) && strncmp("ttyUSB0", entry->d_name, 7) != 0)
00273                 {
00274                         sprintf(current.m_portName, "/dev/%s", entry->d_name);
00275                         std::cout << "candidate: " << current.m_portName << std::endl;
00276                         current.m_baudrate = baudrate;
00277                         ports.append(current);
00278                 }
00279         closedir(dir);
00280         
00281         ports.sortAscending();
00282 #endif
00283 
00284         // try to connect so we can detect if there really is an MT / XM attached
00285         unsigned p = 0;
00286         while (!abortScan && p < ports.length())
00287         {
00288                 CmtPortInfo port = ports[p];
00289                 std::cout << port.m_baudrate << "|" << port.m_deviceId << "|" << port.m_portName << "|" << port.m_portNr << std::endl;
00290                 if (cmtScanPort(ports[p],ports[p].m_baudrate,singleScanTimeout,scanTries))
00291                 {
00292                         std::cout << "success" << std::endl;
00293                         ++p;
00294                 }
00295                 else
00296                 {
00297                         ports.remove(p);
00298                         std::cout << "failed" << std::endl;
00299                 }
00300         }
00301 
00302         if (abortScan)
00303                 return abortScan = false;
00304 
00305         // Now sort the final list by ascending port nr
00306         ports.sortAscending();
00307         abortScan = false;
00308         return true;
00309 }
00310 
00311 } // end of xsens namespace
Generated on Sun May 8 08:41:36 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3