예제 #1
0
/*! \brief Enumerate Xsens USB devices

	If the OS already has drivers running for a device, the device should already have been
	found by xsEnumerateSerialPorts().

	\param[in,out] ports The list of serial ports to append to
*/
bool xsEnumerateUsbDevices(XsPortInfoList& ports)
{
	XsPortInfo current;
#ifdef USE_WINUSB
	BOOL bResult = FALSE;
	ULONG length;
	ULONG requiredLength=0;

	// {FD51225C-700A-47e5-9999-B2D9031B88ED}
	GUID guid = { 0xfd51225c, 0x700a, 0x47e5, { 0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed } };

	HDEVINFO deviceInfo;
	SP_DEVICE_INTERFACE_DATA interfaceData;
	PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = NULL;

	deviceInfo = SetupDiGetClassDevs(&guid, NULL, NULL,	DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);

	// Initialize variables.
	interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA);
	int port = 0;
	for (DWORD dwIndex = 0; port == 0; ++dwIndex)
	{
		BOOL bRet = SetupDiEnumDeviceInterfaces( deviceInfo, NULL, &guid, dwIndex, &interfaceData);
		if (!bRet)
		{
			if (GetLastError() == ERROR_NO_MORE_ITEMS)
				break;
		}
		else
		{
			if (!SetupDiGetDeviceInterfaceDetail(deviceInfo, &interfaceData, NULL, 0, &requiredLength, NULL))
			{
				if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
				{
					SetupDiDestroyDeviceInfoList(deviceInfo);
					return false;
				}
			}
			detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(LMEM_FIXED, requiredLength);
			if (NULL == detailData)
			{
				SetupDiDestroyDeviceInfoList(deviceInfo);
				return false;
			}

			detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
			length = requiredLength;
			SP_DEVINFO_DATA DevInfoData;
			DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
			bResult = SetupDiGetDeviceInterfaceDetailA(deviceInfo, &interfaceData, detailData, length, &requiredLength, &DevInfoData);

			if (!bResult)
			{
				LocalFree(detailData);
				SetupDiDestroyDeviceInfoList(deviceInfo);
				return false;
			}

			unsigned char serialNumber[256];
			char* ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#');
			if (!ptrStart)
				continue;
			ptrStart = strchr(ptrStart+1, '#');
			if (!ptrStart)
				continue;
			ptrEnd = strchr(ptrStart+1, '#');
			if (!ptrEnd)
				continue;

			strncpy((char*)serialNumber, ptrStart+1, ptrEnd-ptrStart-1);
			serialNumber[ptrEnd-ptrStart-1] = '\0';

			current.setPortName(detailData->DevicePath);

			int id = 0;
			sscanf((const char *)serialNumber, "%X", &id);
			current.setDeviceId((uint32_t) id);

			ports.push_back(current);
		}
	}

	SetupDiDestroyDeviceInfoList(deviceInfo);
	return true;
#elif defined(HAVE_LIBUSB)
	XsLibUsb libUsb;
	libusb_context *context;
	int result = libUsb.init(&context);
	if (result != LIBUSB_SUCCESS)
		return true;

	libusb_device **deviceList;
	ssize_t deviceCount = libUsb.get_device_list(context, &deviceList);
	for (ssize_t i = 0; i < deviceCount; i++)
	{
		libusb_device *device = deviceList[i];
		libusb_device_descriptor desc;
		result = libUsb.get_device_descriptor(device, &desc);
		if (result != LIBUSB_SUCCESS)
			continue;

		if (desc.idVendor != XSENS_VENDOR_ID && desc.idVendor != ATMEL_VENDOR_ID)
			continue;

		libusb_device_handle *handle;
		result = libUsb.open(device, &handle);
		if (result != LIBUSB_SUCCESS)
			continue;

		unsigned char serialNumber[256];
		result = libUsb.get_string_descriptor_ascii(handle, desc.iSerialNumber, serialNumber, 256);

		if (desc.idVendor == ATMEL_VENDOR_ID && desc.idProduct == ATMEL_BORROWED_PRODUCT_ID)
		{
			unsigned char productName[256];
			result = libUsb.get_string_descriptor_ascii(handle, desc.iProduct, productName, 256);

			if (strcmp("Xsens COM port", (const char *)productName) != 0)
			{
				libUsb.close(handle);
				continue;
			}
		}

		libusb_config_descriptor *configDesc;
		result = libUsb.get_active_config_descriptor(device, &configDesc);
		if (result != LIBUSB_SUCCESS)
		{
			libUsb.close(handle);
			continue;
		}

		bool kernelActive = false;
		for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces; ++ifCount) {
			int res = libUsb.kernel_driver_active(handle, ifCount);
			kernelActive |= (res == 1);
		}

		libUsb.free_config_descriptor(configDesc);

		if (!kernelActive)
		{
			char name[256];
			sprintf(name, "USB%03u:%03u", libUsb.get_bus_number(device),
								libUsb.get_device_address(device));
			current.setPortName(name);

			int id = 0;
			sscanf((const char *)serialNumber, "%X", &id);
			current.setDeviceId((uint32_t) id);
			ports.push_back(current);
		}
		else
		{
			JLDEBUG(gJournal, "Kernel driver active on USB" <<
				libUsb.get_bus_number(device) << ":" << libUsb.get_device_address(device) <<
					" device " << serialNumber);

		}
		libUsb.close(handle);
	}
	libUsb.free_device_list(deviceList, 1);
	libUsb.exit(context);
	return true;
#else
	(void)ports;
	return false;
#endif
}
예제 #2
0
/*-------------------------------------------------------------
					initialize
-------------------------------------------------------------*/
void CIMUXSens_MT4::initialize()
{
#if MRPT_HAS_xSENS_MT4
	m_state = ssInitializing;

	try
	{
		// Try to open a specified device, or scan the bus?
		XsPortInfoArray portInfoArray;

		if (m_portname.empty())
		{
			if (m_verbose) cout << "[CIMUXSens_MT4] Scanning for USB devices...\n";
			xsEnumerateUsbDevices(portInfoArray);

			if (portInfoArray.empty())
				THROW_EXCEPTION("CIMUXSens_MT4: No 'portname' was specified and no XSens device was found after scanning the system!")

			if (m_verbose) cout << "[CIMUXSens_MT4] Found " <<  portInfoArray.size() <<" devices. Opening the first one.\n";
		}
		else
		{
			XsPortInfo portInfo(m_portname, XsBaud::numericToRate(m_port_bauds));
			if (m_verbose) cout << "[CIMUXSens_MT4] Using user-supplied portname '"<<m_portname<<"' at "<<m_port_bauds<<" baudrate.\n";
			portInfoArray.push_back(portInfo);
		}

		// Use the first detected device
		XsPortInfo mtPort = portInfoArray.at(0);

		// Open the port with the detected device
		cout << "[CIMUXSens_MT4] Opening port " << mtPort.portName().toStdString() << std::endl;

		if (!my_xsens_device.openPort(mtPort))
			throw std::runtime_error("Could not open port. Aborting.");

		// Put the device in configuration mode
		if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into configuration mode...\n";
		if (!my_xsens_device.gotoConfig()) // Put the device into configuration mode before configuring the device
			throw std::runtime_error("Could not put device into configuration mode. Aborting.");

		// Request the device Id to check the device type
		mtPort.setDeviceId(my_xsens_device.getDeviceId());

		my_xsens_devid = mtPort.deviceId();

		// Check if we have an MTi / MTx / MTmk4 device
		if (!mtPort.deviceId().isMtix() && !mtPort.deviceId().isMtMk4())
		{
			throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
		}
		cout << "[CIMUXSens_MT4] Found a device with id: " << mtPort.deviceId().toString().toStdString() << " @ port: " << mtPort.portName().toStdString() << ", baudrate: " << mtPort.baudrate() << std::endl;

		// Print information about detected MTi / MTx / MTmk4 device
		if (m_verbose) cout << "[CIMUXSens_MT4] Device: " << my_xsens_device.getProductCode().toStdString() << " opened." << std::endl;

		// Configure the device. Note the differences between MTix and MTmk4
		if (m_verbose) cout << "[CIMUXSens_MT4] Configuring the device..." << std::endl;
		if (mtPort.deviceId().isMtix())
		{
			XsOutputMode outputMode = XOM_Orientation; // output orientation data
			XsOutputSettings outputSettings = XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All; // XOS_OrientationMode_Quaternion; // output orientation data as quaternion

			// set the device configuration
			if (!my_xsens_device.setDeviceMode(outputMode, outputSettings))
				throw std::runtime_error("Could not configure MT device. Aborting.");
		}
		else if (mtPort.deviceId().isMtMk4())
		{
			XsOutputConfigurationArray configArray;
			configArray.push_back( XsOutputConfiguration(XDI_SampleTime64,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_SampleTimeFine,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_SampleTimeCoarse,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_Quaternion,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_Temperature,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_Acceleration,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_RateOfTurn,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_MagneticField,m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_VelocityXYZ,m_sampleFreq) );

			configArray.push_back( XsOutputConfiguration(XDI_StatusByte, m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_LatLon, m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_UtcTime, m_sampleFreq) );
			configArray.push_back( XsOutputConfiguration(XDI_AltitudeEllipsoid, m_sampleFreq) );

			if (!my_xsens_device.setOutputConfiguration(configArray))
				throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
		}
		else
		{
			throw std::runtime_error("Unknown device while configuring. Aborting.");
		}

		// Put the device in measurement mode
		if (m_verbose) cout << "[CIMUXSens_MT4] Putting device into measurement mode..." << std::endl;
		if (!my_xsens_device.gotoMeasurement())
			throw std::runtime_error("Could not put device into measurement mode. Aborting.");

		m_state = ssWorking;

	}