/*! \brief Open an IO device
\param portInfo The info to use for opening the port
\return True when successful
*/
bool openPort(const XsPortInfo& portInfo)
{
if (portInfo.isUsb())
m_streamInterface = new UsbInterface();
else
m_streamInterface = new SerialInterface();
if (m_streamInterface->open(portInfo) != XRV_OK) return false;
return true;
}
/*! \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
}
/*-------------------------------------------------------------
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;
}
/*! \brief Open a communication channel to the given USB port name. */
XsResultValue UsbInterface::open(const XsPortInfo &portInfo, uint32_t, uint32_t)
{
d->m_endTime = 0;
#ifdef USE_WINUSB
JLDEBUG(gJournal, "Open usb port " << portInfo.portName().toStdString());
#else
JLDEBUG(gJournal, "Open usb port " << portInfo.usbBus() << ":" << portInfo.usbAddress());
#endif
if (isOpen())
{
JLALERT(gJournal, "Port " << portInfo.portName().toStdString() << " already open");
return (d->m_lastResult = XRV_ALREADYOPEN);
}
#ifdef USE_WINUSB
d->m_deviceHandle = CreateFileA(portInfo.portName().c_str(),
GENERIC_WRITE | GENERIC_READ,
FILE_SHARE_WRITE | FILE_SHARE_READ,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED,
NULL);
if (d->m_deviceHandle == INVALID_HANDLE_VALUE)
{
d->m_deviceHandle = NULL;
return (d->m_lastResult = XRV_PORTNOTFOUND);
}
BOOL result = FALSE;
UCHAR speed = 0;
ULONG length = 0;
USB_INTERFACE_DESCRIPTOR interfaceDescriptor = {0,0,0,0,0,0,0,0,0};
WINUSB_PIPE_INFORMATION pipeInfo;
result = d->m_winUsb.Initialize(d->m_deviceHandle, &d->m_usbHandle[0]);
if (result)
{
result = d->m_winUsb.GetAssociatedInterface(d->m_usbHandle[0],0,&d->m_usbHandle[1]);
}
else
{
#ifdef XSENS_DEBUG
DWORD err = GetLastError();
assert(result);
#endif
return (d->m_lastResult = XRV_ERROR);
}
for (int k = 0; k<2;k++)
{
if(result)
{
assert(d->m_usbHandle[k] != 0);
length = sizeof(UCHAR);
result = d->m_winUsb.QueryDeviceInformation(d->m_usbHandle[k],
DEVICE_SPEED,
&length,
&speed);
}
if(result)
{
d->m_deviceSpeed = speed;
result = d->m_winUsb.QueryInterfaceSettings(d->m_usbHandle[k],
0,
&interfaceDescriptor);
}
if(result)
{
for(int i=0;i<interfaceDescriptor.bNumEndpoints;i++)
{
result = d->m_winUsb.QueryPipe(d->m_usbHandle[k],
0,
(UCHAR) i,
&pipeInfo);
if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId))
{
d->m_bulkInPipe = pipeInfo.PipeId;
d->m_bulkInPipePacketSize =
pipeInfo.MaximumPacketSize;
}
else if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
USB_ENDPOINT_DIRECTION_OUT(pipeInfo.PipeId))
{
d->m_bulkOutPipe = pipeInfo.PipeId;
}
else if(pipeInfo.PipeType == UsbdPipeTypeInterrupt)
{
d->m_interruptPipe = pipeInfo.PipeId;
}
else
{
result = FALSE;
break;
}
}
}
}
setTimeout(0); //lint !e534
flushData(); //lint !e534
sprintf(d->m_portname, "%s", portInfo.portName().c_str());
// d->m_offset = 0;
::ResetEvent(&d->m_quitEvent); //lint !e534
d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId);
if (d->m_threadHandle == XSENS_INVALID_THREAD)
{
#ifdef XSENS_DEBUG
assert(0);
#endif
return (d->m_lastResult = XRV_ERROR);
}
#else // !USE_WINUSB
libusb_device **deviceList;
ssize_t listLength = UsbInterfacePrivate::getContextManager().m_libUsb.get_device_list(UsbInterfacePrivate::getContextManager().m_usbContext, &deviceList);
if (listLength < 0)
return d->m_lastResult = d->libusbErrorToXrv((int)listLength);
// "USBxxx:yyy"
uint8_t bus = XsPortInfo_usbBus(&portInfo);
uint8_t address = XsPortInfo_usbAddress(&portInfo);
XsResultValue xrv = XRV_OK;
int result;
libusb_device *device = NULL;
for (int i = 0; i < listLength && device == NULL; ++i) {
libusb_device *dev = deviceList[i];
if (UsbInterfacePrivate::getContextManager().m_libUsb.get_bus_number(dev) != bus || UsbInterfacePrivate::getContextManager().m_libUsb.get_device_address(dev) != address)
continue;
libusb_device_descriptor desc;
result = UsbInterfacePrivate::getContextManager().m_libUsb.get_device_descriptor(dev, &desc);
if (result != LIBUSB_SUCCESS)
break;
libusb_config_descriptor *configDesc;
result = UsbInterfacePrivate::getContextManager().m_libUsb.get_active_config_descriptor(dev, &configDesc);
if (result != LIBUSB_SUCCESS)
break;
d->m_interface = -1;
d->m_interfaceCount = configDesc->bNumInterfaces;
// find the bulk transfer endpoints
for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces && d->m_interface == -1; ++ifCount) {
for (uint8_t altsettingCount = 0; altsettingCount < configDesc->interface[ifCount].num_altsetting; altsettingCount++) {
const libusb_endpoint_descriptor *endpoints = configDesc->interface[ifCount].altsetting[altsettingCount].endpoint;
int inEndpoint = -1, outEndpoint = -1;
for (uint8_t i = 0; i < configDesc->interface[ifCount].altsetting[altsettingCount].bNumEndpoints; i++) {
if ((endpoints[i].bmAttributes&LIBUSB_TRANSFER_TYPE_MASK) != LIBUSB_TRANSFER_TYPE_BULK)
continue;
switch (endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_DIR_MASK) {
case LIBUSB_ENDPOINT_IN:
inEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK;
break;
case LIBUSB_ENDPOINT_OUT:
outEndpoint = endpoints[i].bEndpointAddress&LIBUSB_ENDPOINT_ADDRESS_MASK;
break;
}
}
if (outEndpoint == -1 || inEndpoint == -1)
continue;
d->m_interface = ifCount;
d->m_dataOutEndPoint = outEndpoint;
d->m_dataInEndPoint = inEndpoint;
}
}
if (d->m_interface == -1) {
xrv = XRV_INPUTCANNOTBEOPENED;
break;
}
UsbInterfacePrivate::getContextManager().m_libUsb.free_config_descriptor(configDesc);
UsbInterfacePrivate::getContextManager().m_libUsb.ref_device(dev);
device = dev;
result = LIBUSB_SUCCESS;
}
UsbInterfacePrivate::getContextManager().m_libUsb.free_device_list(deviceList, 1);
if (result != LIBUSB_SUCCESS) {
UsbInterfacePrivate::getContextManager().m_libUsb.unref_device(device);
return d->m_lastResult = d->libusbErrorToXrv(result);
}
if (xrv != XRV_OK) {
UsbInterfacePrivate::getContextManager().m_libUsb.unref_device(device);
return d->m_lastResult = xrv;
}
libusb_device_handle *handle;
result = UsbInterfacePrivate::getContextManager().m_libUsb.open(device, &handle);
if (result != LIBUSB_SUCCESS) {
UsbInterfacePrivate::getContextManager().m_libUsb.unref_device(device);
return d->m_lastResult = d->libusbErrorToXrv(result);
}
// be rude and claim all interfaces
for (int i = 0; i < d->m_interfaceCount; i++) {
result = UsbInterfacePrivate::getContextManager().m_libUsb.kernel_driver_active(handle, i);
if (result > 0)
result = UsbInterfacePrivate::getContextManager().m_libUsb.detach_kernel_driver(handle, i);
if (result == LIBUSB_SUCCESS)
result = UsbInterfacePrivate::getContextManager().m_libUsb.claim_interface(handle, i);
if (result != LIBUSB_SUCCESS) {
for (int j = 0; j < i; j++) {
while (result != LIBUSB_SUCCESS) {
result = UsbInterfacePrivate::getContextManager().m_libUsb.release_interface(handle, j);
UsbInterfacePrivate::getContextManager().m_libUsb.attach_kernel_driver(handle, j);
}
}
UsbInterfacePrivate::getContextManager().m_libUsb.close(handle);
UsbInterfacePrivate::getContextManager().m_libUsb.unref_device(device);
return d->m_lastResult = d->libusbErrorToXrv(result);
}
}
d->m_deviceHandle = handle;
sprintf(d->m_portname, "%s", portInfo.portName().c_str());
flushData();
#endif // !USE_WINUSB
JLDEBUG(gJournal, "USB Port opened");
return (d->m_lastResult = XRV_OK);
}
/*! \brief Open a communication channel to the given port info.
\details If the baudrate in \a portInfo is set to XBR_Invalid, the baud rate is automatically
detected if possible.
\param portInfo The details of the port that should be opened. Depending on the type of interface,
parts of this parameter may be ignored.
\param readBufSize The size of the read buffer in bytes (if appliccable to the device)
\param writeBufSize The size of the write buffer in bytes (if appliccable to the device)
\returns XRV_OK if the device was opened successfully
*/
XsResultValue SerialInterface::open(const XsPortInfo& portInfo,
uint32_t readBufSize,
uint32_t writeBufSize)
{
m_endTime = 0;
JLDEBUG(gJournal, "port " << portInfo.portName().toStdString() << " at " << portInfo.baudrate() << " baud");
if (isOpen())
{
JLALERT(gJournal, "Port " << portInfo.portName().toStdString() << " is already open");
return (m_lastResult = XRV_ALREADYOPEN);
}
m_baudrate = portInfo.baudrate();
#ifdef _WIN32
XsResultValue fail = XRV_OK;
char winPortName[32];
// Open port
sprintf(winPortName, "\\\\.\\%s", portInfo.portName().c_str());
m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL,
OPEN_EXISTING, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
JLDEBUG(gJournal, "Port " << portInfo.portName().toStdString() << " cannot be opened");
return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
}
DCB commState; //!< Stored settings about the serial port
commState.DCBlength = sizeof(DCB);
//Get the current state & then change it
if (!GetCommState(m_handle, &commState)) // Get current state
fail = XRV_ERROR;
commState.BaudRate = (int) portInfo.baudrate(); // Setup the baud rate
commState.Parity = NOPARITY; // Setup the Parity
commState.ByteSize = 8; // Setup the data bits
commState.StopBits = TWOSTOPBITS; // Setup the stop bits
commState.fDsrSensitivity = FALSE; // Setup the flow control
commState.fOutxCtsFlow = FALSE; // NoFlowControl:
commState.fOutxDsrFlow = FALSE;
commState.fOutX = FALSE;
commState.fInX = FALSE;
if (!SetCommState(m_handle, (LPDCB)&commState)) // Set new state
{
// Bluetooth ports cannot always be opened with 2 stopbits
// Now try to open port with 1 stopbit.
commState.StopBits = ONESTOPBIT;
if (!SetCommState(m_handle, (LPDCB)&commState))
fail = XRV_INPUTCANNOTBEOPENED;
}
std::string tmp = portInfo.portName().toStdString();
m_port = atoi(&tmp.c_str()[3]);
sprintf(m_portname, "%s", tmp.c_str());
if (setTimeout(20))
fail = m_lastResult;
// Other initialization functions
if (!EscapeCommFunction(m_handle, SETRTS)) // Enable RTS (for Xbus Master use)
fail = XRV_ERROR;
if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise
fail = XRV_ERROR;
if (!SetupComm(m_handle,readBufSize,writeBufSize)) // Set queue size
fail = XRV_ERROR;
// Remove any 'old' data in buffer
//PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
if (!PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR))
fail = XRV_ERROR;
if (fail != XRV_OK)
{
CloseHandle(m_handle); //lint !e534
m_handle = INVALID_HANDLE_VALUE;
return (m_lastResult = fail);
}
#else // !_WIN32
(void)readBufSize;
(void)writeBufSize;
// Open port
std::string pn = portInfo.portName().toStdString();
m_handle = ::open(pn.c_str(), O_RDWR | O_NOCTTY);
// O_RDWR: Read+Write
// O_NOCTTY: Raw input, no "controlling terminal"
// O_NDELAY: Don't care about DCD signal
if (m_handle < 0) {
// Port not open
return m_lastResult = XRV_INPUTCANNOTBEOPENED;
}
// Check if the file is already opened by someome else (other thread/process)
if (flock(m_handle, LOCK_EX | LOCK_NB))
{
closeLive();
return m_lastResult = XRV_INPUTCANNOTBEOPENED;
}
/* Start configuring of port for non-canonical transfer mode */
// Get current options for the port
if (tcgetattr(m_handle, &m_commState) != 0)
return XRV_ERROR;
// Set baudrate.
if (cfsetispeed(&m_commState, portInfo.baudrate()) != 0)
return XRV_ERROR;
if (cfsetospeed(&m_commState, portInfo.baudrate()) != 0)
return XRV_ERROR;
// Enable the receiver and set local mode
m_commState.c_cflag |= (CLOCAL | CREAD);
// Set character size to data bits and set no parity Mask the characte size bits
m_commState.c_cflag &= ~(CSIZE|PARENB);
m_commState.c_cflag |= CS8; // Select 8 data bits
m_commState.c_cflag |= CSTOPB; // send 2 stop bits
// Disable hardware flow control
m_commState.c_cflag &= ~CRTSCTS;
m_commState.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
// Disable software flow control
m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
// Set Raw output
m_commState.c_oflag &= ~OPOST;
// Timeout 0.001 sec for first byte, read minimum of 0 bytes
m_commState.c_cc[VMIN] = 0;
m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1
// Set the new options for the port
if (tcsetattr(m_handle,TCSANOW, &m_commState) != 0)
return XRV_INPUTCANNOTBEOPENED;
termios checkCommState;
if (tcgetattr(m_handle, &checkCommState) != 0)
return XRV_ERROR;
if ((m_commState.c_cflag != checkCommState.c_cflag) ||
(m_commState.c_iflag != checkCommState.c_iflag) ||
(m_commState.c_oflag != checkCommState.c_oflag) ||
(m_commState.c_cc[VMIN] != checkCommState.c_cc[VMIN]) ||
(m_commState.c_cc[VTIME] != checkCommState.c_cc[VTIME]))
{
JLDEBUG(gJournal, "commstates do not match, which is OK for USB connected MkIV devices");
}
m_port = 1;
sprintf(m_portname, "%s", pn.c_str());
tcflush(m_handle, TCIOFLUSH);
// setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors
int cmbits;
if (ioctl(m_handle, TIOCMGET, &cmbits) < 0)
{
JLDEBUG(gJournal, "TIOCMGET failed, which is OK for USB connected MkIV devices");
}
cmbits |= TIOCM_RTS|TIOCM_DTR;
if (ioctl(m_handle, TIOCMSET, &cmbits) < 0)
{
JLDEBUG(gJournal, "TIOCMSET failed, which is OK for USB connected MkIV devices");
}
#endif // !_WIN32
JLDEBUG(gJournal, "Port " << portInfo.portName().toStdString() << " opened");
return (m_lastResult = XRV_OK);
}
/*! \brief Open a communication channel to the given port info.
\details If the baudrate in \a portInfo is set to XBR_Invalid, the baud rate is automatically
detected if possible.
\param portInfo The details of the port that should be opened. Depending on the type of interface,
parts of this parameter may be ignored.
\param readBufSize The size of the read buffer in bytes (if appliccable to the device)
\param writeBufSize The size of the write buffer in bytes (if appliccable to the device)
\param options The options to enable (flow control, stop bits)
\returns XRV_OK if the device was opened successfully
*/
XsResultValue SerialInterface::open(const XsPortInfo& portInfo,
uint32_t readBufSize,
uint32_t writeBufSize,
PortOptions options)
{
m_endTime = 0;
JLDEBUG(gJournal, portInfo);
if (isOpen())
{
JLALERT(gJournal, "Port " << portInfo.portName() << " is already open");
return (m_lastResult = XRV_ALREADYOPEN);
}
m_baudrate = portInfo.baudrate();
if (options&PO_RtsCtsFlowControl)
JLTRACE(gJournal, "Requested RTS/CTS flow control");
if (options&PO_DtrDsrFlowControl)
JLTRACE(gJournal, "Requested DTR/DSR flow control");
if (options&PO_XonXoffFlowControl)
JLTRACE(gJournal, "Requested Xon/Xoff flow control");
#ifdef _WIN32
XsResultValue fail = XRV_OK;
char winPortName[256];
// Open port
sprintf(winPortName, "\\\\.\\%s", portInfo.portName().c_str());
m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL,
OPEN_EXISTING, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
JLDEBUG(gJournal, "Port " << portInfo.portName() << " cannot be opened");
return (m_lastResult = XRV_INPUTCANNOTBEOPENED);
}
DCB commState; //!< Stored settings about the serial port
commState.DCBlength = sizeof(DCB);
//Get the current state & then change it
if (!GetCommState(m_handle, &commState)) // Get current state
fail = XRV_ERROR;
commState.BaudRate = (int) portInfo.baudrate(); // Setup the baud rate
commState.Parity = NOPARITY; // Setup the Parity
commState.ByteSize = 8; // Setup the data bits
commState.StopBits = (options&PO_TwoStopBits)?TWOSTOPBITS:ONESTOPBIT;
// Setup flow control
commState.fDsrSensitivity = (options&PO_DtrDsrFlowControl)?TRUE:FALSE;
commState.fOutxDsrFlow = (options&PO_DtrDsrFlowControl)?DTR_CONTROL_HANDSHAKE:DTR_CONTROL_DISABLE;
commState.fOutxCtsFlow = (options&PO_RtsCtsFlowControl)?TRUE:FALSE;
commState.fRtsControl = (options&PO_RtsCtsFlowControl)?RTS_CONTROL_HANDSHAKE:RTS_CONTROL_ENABLE;
commState.fOutX = (options&PO_XonXoffFlowControl)?TRUE:FALSE;
commState.fInX = commState.fOutX;
if (!SetCommState(m_handle, (LPDCB)&commState)) // Set new state
{
// Bluetooth ports cannot always be opened with 2 stopbits
// Now try to open port with 1 stopbit.
commState.StopBits = ONESTOPBIT;
if (!SetCommState(m_handle, (LPDCB)&commState))
fail = XRV_INPUTCANNOTBEOPENED;
}
std::string tmp = portInfo.portName().toStdString();
m_port = atoi(&tmp.c_str()[3]);
sprintf(m_portname, "%s", tmp.c_str());
if (setTimeout(20))
fail = m_lastResult;
// Other initialization functions
if ((options&PO_DtrDsrFlowControl) == 0)
{
if (!EscapeCommFunction(m_handle, SETDTR)) // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise
fail = XRV_ERROR;
}
if (!SetupComm(m_handle,readBufSize,writeBufSize)) // Set queue size
fail = XRV_ERROR;
// Remove any 'old' data in buffer
//PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
if (!PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR))
fail = XRV_ERROR;
if (fail != XRV_OK)
{
CloseHandle(m_handle); //lint !e534
m_handle = INVALID_HANDLE_VALUE;
return (m_lastResult = fail);
}
#else // !_WIN32
(void)readBufSize;
(void)writeBufSize;
// Open port
std::string pn = portInfo.portName().toStdString();
m_handle = ::open(pn.c_str(), O_RDWR | O_NOCTTY);
// O_RDWR: Read+Write
// O_NOCTTY: Raw input, no "controlling terminal"
// O_NDELAY: Don't care about DCD signal
if (m_handle < 0) {
// Port not open
return m_lastResult = XRV_INPUTCANNOTBEOPENED;
}
// Check if the file is already opened by someome else (other thread/process)
if (flock(m_handle, LOCK_EX | LOCK_NB))
{
closeLive();
return m_lastResult = XRV_INPUTCANNOTBEOPENED;
}
/* Start configuring of port for non-canonical transfer mode */
// Get current options for the port
if (tcgetattr(m_handle, &m_commState) != 0)
return XRV_ERROR;
// Set baudrate.
if (cfsetispeed(&m_commState, portInfo.baudrate()) != 0)
return XRV_ERROR;
if (cfsetospeed(&m_commState, portInfo.baudrate()) != 0)
return XRV_ERROR;
// Enable the receiver and set local mode
m_commState.c_cflag |= (CLOCAL | CREAD);
// Set character size to data bits and set no parity Mask the characte size bits
m_commState.c_cflag &= ~(CSIZE|PARENB|PARODD);
m_commState.c_cflag |= CS8; // Select 8 data bits
m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CSTOPB, (options&PO_TwoStopBits) == PO_TwoStopBits);
// Hardware flow control
m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CRTSCTS, (options&PO_RtsCtsFlowControl) == PO_RtsCtsFlowControl);
#ifdef CDTRDSR
m_commState.c_cflag = setBitsEnabled(m_commState.c_cflag, (tcflag_t)CDTRDSR, (options&PO_DtrDsrFlowControl) == PO_DtrDsrFlowControl);
#endif
m_commState.c_lflag &= ~(ECHO|ECHOE|ECHOK|ECHONL|ICANON|ISIG|IEXTEN);
// Software flow control
m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR|ICRNL);
m_commState.c_iflag = setBitsEnabled(m_commState.c_iflag, (tcflag_t)(IXON|IXOFF), options&PO_XonXoffFlowControl);
// Set Raw output
m_commState.c_oflag &= ~OPOST;
// Timeout 0.001 sec for first byte, read minimum of 0 bytes
m_commState.c_cc[VMIN] = 0;
m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1
// Set the new options for the port
if (tcsetattr(m_handle,TCSANOW, &m_commState) != 0)
return XRV_INPUTCANNOTBEOPENED;
#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_ALERT
termios checkCommState;
if (tcgetattr(m_handle, &checkCommState) != 0)
return XRV_ERROR;
if (cfgetispeed(&checkCommState) != portInfo.baudrate())
JLALERT(gJournal, "Set baudrate doesn't match requested baudrate");
if (cfgetospeed(&checkCommState) != portInfo.baudrate())
JLALERT(gJournal, "Set baudrate doesn't match requested baudrate");
if (options&PO_RtsCtsFlowControl && !(checkCommState.c_cflag&CRTSCTS))
JLALERT(gJournal, "Requested RTS/CTS flow control, but could not be set.");
if (options&PO_DtrDsrFlowControl &&
#ifdef CDTRDSR
!(checkCommState.c_cflag&CDTRDSR)
#else
false
#endif
)
JLALERT(gJournal, "Requested DTR/DSR flow control, but could not be set.");
if (options&PO_XonXoffFlowControl && !((checkCommState.c_iflag&(IXON|IXOFF)) == (IXON|IXOFF)))
JLALERT(gJournal, "Requested Xon/Xoff flow control, but could not be set.");
#endif // JLLOGLEVEL < JLL_ALERT
#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_DEBUG
#define CHECK_COMMSTATE(req, res, field)\
if (req.field != res.field) \
{\
JLDEBUG(gJournal, "field " << #field << " does not match");\
JLDEBUG(gJournal, "actual : " << std::oct << (uint64_t)res.field);\
JLDEBUG(gJournal, "expected: " << std::oct << (uint64_t)req.field);\
}
#else
#define CHECK_COMMSTATE(req, res, field)
#endif
CHECK_COMMSTATE(m_commState, checkCommState, c_cflag);
CHECK_COMMSTATE(m_commState, checkCommState, c_iflag);
CHECK_COMMSTATE(m_commState, checkCommState, c_oflag);
CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VMIN]);
CHECK_COMMSTATE(m_commState, checkCommState, c_cc[VTIME]);
m_port = 1;
sprintf(m_portname, "%s", pn.c_str());
tcflush(m_handle, TCIOFLUSH);
// setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors
int cmbits;
if (ioctl(m_handle, TIOCMGET, &cmbits) < 0)
{
JLDEBUG(gJournal, "TIOCMGET failed, which is OK for USB connected MkIV devices");
}
if ((options&PO_RtsCtsFlowControl) == 0)
cmbits = setBitsEnabled(cmbits, TIOCM_RTS, true);
// else don't touch them
cmbits = setBitsEnabled(cmbits, TIOCM_DTR, !(options&PO_DtrDsrFlowControl));
if (ioctl(m_handle, TIOCMSET, &cmbits) < 0)
{
JLDEBUG(gJournal, "TIOCMSET failed, which is OK for USB connected MkIV devices");
}
#endif // !_WIN32
JLDEBUG(gJournal, "Port " << portInfo.portName().toStdString() << " opened");
return (m_lastResult = XRV_OK);
}