////////////////////////////////////////////////////////////////////////////////////////// // Wait for data to arrive or a timeout to occur. XsensResultValue Cmt1s::waitForData (const uint32_t maxLength, uint8_t* data, uint32_t* length) { CMT1LOG("L1: waitForData, mto=%u, length=%p\n",m_timeout,length); uint32_t timeout = m_timeout; uint32_t ln; if (length == NULL) length = &ln; uint32_t eTime = getTimeOfDay(NULL) + timeout; uint32_t newLength = 0; *length = 0; while ((*length < maxLength) && (getTimeOfDay() <= eTime)) { readData(maxLength - *length, data + *length, &newLength); *length += newLength; } CMT1LOG("L1: waitForData result: read %u of %u bytes\n",length[0],maxLength); if (length[0] < maxLength) return (m_lastResult = XRV_TIMEOUT); else return (m_lastResult = XRV_OK); }
////////////////////////////////////////////////////////////////////////////////////////// // Read data from the serial port and put it into the data buffer. XsensResultValue Cmt1s::readData( const uint32_t maxLength, uint8_t* data, uint32_t* length) { CMT1LOG("L1: readData, maxlength=%u, length=%p\n", maxLength, length); uint32_t ln; if (length == nullptr) length = &ln; if (!m_isOpen) return (m_lastResult = XRV_NOPORTOPEN); #ifdef _WIN32 BOOL rres = ::ReadFile(m_handle, data, maxLength, (DWORD*)length, nullptr); if (m_onBytesReceived != nullptr && *length > 0) { CmtBinaryData* bytes = (CmtBinaryData*)malloc(sizeof(CmtBinaryData)); bytes->m_size = *length; bytes->m_portNr = m_port; memcpy(bytes->m_data, data, *length); #ifdef _LOG_CALLBACKS CMTLOG( "C1: onBytesReceived(%d,(%d,%d),%p)\n", (int32_t)m_onBytesReceivedInstance, (int32_t)bytes->m_size, (int32_t)bytes->m_portNr, m_onBytesReceivedParam); #endif m_onBytesReceived( m_onBytesReceivedInstance, CMT_CALLBACK_ONBYTESRECEIVED, bytes, m_onBytesReceivedParam); } if (!rres) { CMT1LOG("L1: readData, ReadFile returned error %u\n", ::GetLastError()); return (m_lastResult = XRV_ERROR); } #else *length = read(m_handle, data, maxLength); #endif #ifdef _LOG_RX_TX if (*length > 0) { if (rx_log == nullptr) { char fname[CMT_MAX_FILENAME_LENGTH]; sprintf(fname, "rx_%03d_%d.log", (int32_t)m_port, m_baudrate); rx_log = fopen(fname, "wb"); } fwrite(data, 1, *length, rx_log); } #endif CMT1LOG( (length[0] ? "L1: readData returned success, read %u of %u bytes, " "first: %02x\n" : "L1: readData returned success, read %u bytes\n"), length[0], maxLength, data[0]); return (m_lastResult = XRV_OK); }
////////////////////////////////////////////////////////////////////////////////////////// // Set the default timeout value to use in blocking operations. XsensResultValue Cmt1s::setTimeout (const uint32_t ms) { CMT1LOG("L1: Setting timeout to %u ms\n",ms); m_timeout = ms; #ifdef _WIN32 // Set COM timeouts COMMTIMEOUTS commTimeouts; GetCommTimeouts(m_handle,&commTimeouts); // Fill CommTimeouts structure // immediate return if data is available, wait 1ms otherwise if (m_timeout > 0) { commTimeouts.ReadIntervalTimeout = 0; commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time commTimeouts.ReadTotalTimeoutMultiplier = 0; commTimeouts.WriteTotalTimeoutConstant = m_timeout; commTimeouts.WriteTotalTimeoutMultiplier = 0; } else { // immediate return whether data is available or not commTimeouts.ReadIntervalTimeout = MAXDWORD; commTimeouts.ReadTotalTimeoutConstant = 0; commTimeouts.ReadTotalTimeoutMultiplier = 0; commTimeouts.WriteTotalTimeoutConstant = 0; commTimeouts.WriteTotalTimeoutMultiplier = 0; } SetCommTimeouts(m_handle, &commTimeouts); // Set CommTimeouts structure #else // Timeout 0.1 sec for first byte, read minimum of 0 bytes m_commState.c_cc[VMIN] = 0; m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time // Set the new options for the port if it is open if (m_isOpen) tcsetattr(m_handle,TCSANOW, &m_commState); #endif return (m_lastResult = XRV_OK); }
////////////////////////////////////////////////////////////////////////////////////////// // Open a communication channel to the given serial port name. XsensResultValue Cmt1s::open( const char *portName, const uint32_t baudRate, uint32_t readBufSize, uint32_t writeBufSize) { MRPT_UNUSED_PARAM(readBufSize); MRPT_UNUSED_PARAM(writeBufSize); m_endTime = 0; CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate); if (m_isOpen) { CMT1LOG("L1: Port already open\n"); return (m_lastResult = XRV_ALREADYOPEN); } m_baudrate = baudRate; #ifdef _WIN32 char winPortName[32]; // Open port sprintf(winPortName, "\\\\.\\%s", portName); m_handle = CreateFileA(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); if (m_handle == INVALID_HANDLE_VALUE) { CMT1LOG("L1: Port cannot be opened\n"); return (m_lastResult = XRV_INPUTCANNOTBEOPENED); } // Once here, port is open m_isOpen = true; //Get the current state & then change it GetCommState(m_handle, &m_commState); // Get current state m_commState.BaudRate = baudRate; // Setup the baud rate m_commState.Parity = NOPARITY; // Setup the Parity m_commState.ByteSize = 8; // Setup the data bits m_commState.StopBits = TWOSTOPBITS; // Setup the stop bits m_commState.fDsrSensitivity = FALSE; // Setup the flow control m_commState.fOutxCtsFlow = FALSE; // NoFlowControl: m_commState.fOutxDsrFlow = FALSE; m_commState.fOutX = FALSE; m_commState.fInX = FALSE; if (!SetCommState(m_handle, (LPDCB)&m_commState)) {// Set new state // Bluetooth ports cannot always be opened with 2 stopbits // Now try to open port with 1 stopbit. m_commState.StopBits = ONESTOPBIT; if (!SetCommState(m_handle, (LPDCB)&m_commState)) { CloseHandle(m_handle); m_handle = INVALID_HANDLE_VALUE; m_isOpen = false; return (m_lastResult = XRV_INPUTCANNOTBEOPENED); } } m_port = atoi(&portName[3]); sprintf(m_portname, "%s", portName); setTimeout(m_timeout); // Other initialization functions EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use) // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise EscapeCommFunction(m_handle, SETDTR); SetupComm(m_handle,readBufSize,writeBufSize); // Set queue size // Remove any 'old' data in buffer //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); #else // !_WIN32 // Open port m_handle = ::open(portName, 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; } // Once here, port is open m_isOpen = true; /* Start configuring of port for non-canonical transfer mode */ // Get current options for the port tcgetattr(m_handle, &m_commState); // Set baudrate. cfsetispeed(&m_commState, baudRate); cfsetospeed(&m_commState, baudRate); // 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 tcsetattr(m_handle,TCSANOW, &m_commState); m_port = 0; sprintf(m_portname, "%s", portName); 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) { return (m_lastResult = XRV_ERROR); } cmbits |= TIOCM_RTS|TIOCM_DTR; if (ioctl(m_handle, TIOCMSET, &cmbits) < 0) { return (m_lastResult = XRV_ERROR); } #endif // !_WIN32 CMT1LOG("L1: Port opened\n"); return (m_lastResult = XRV_OK); }