コード例 #1
0
ファイル: serialinterface.cpp プロジェクト: Aharobot/mrpt
/*! \brief Wait for data to arrive or a timeout to occur.
	\details The function waits until \c maxLength data is available or until a timeout occurs.
	The function returns success if data is available or XsResultValue::TIMEOUT if a
	timeout occurred. A timeout value of 0 indicates that the default timeout stored
	in the class should be used.
	\param maxLength The maximum number of bytes to read before returning
	\param data The buffer to put the read data in.
	\returns XRV_OK if \a maxLength bytes were read, XRV_TIMEOUT if less was read, XRV_TIMEOUTNODATA if nothing was read
*/
XsResultValue SerialInterface::waitForData(XsSize maxLength, XsByteArray& data)
{
	data.clear();
	data.reserve(maxLength);

	//char *data = (char *)&_data[0];
	JLTRACE(gJournal, "timeout=" << m_timeout << ", maxLength=" << maxLength);
	uint32_t timeout = m_timeout;

	uint32_t eTime = XsTime_getTimeOfDay(NULL, NULL) + timeout;
//	uint32_t newLength = 0;

	while ((data.size() < maxLength) && (XsTime_getTimeOfDay(NULL, NULL) <= eTime))
	{
		XsByteArray raw;

		if (readData(maxLength - data.size(), raw) != XRV_OK)
			return m_lastResult;
		data.append(raw);
	}
	JLTRACE(gJournal, "Read " << data.size() << " of " << maxLength << " bytes");

	if (data.size() < maxLength)
		return (m_lastResult = XRV_TIMEOUT);
	else
		return (m_lastResult = XRV_OK);
}
コード例 #2
0
ファイル: serialinterface.cpp プロジェクト: 3660628/mrpt
/*! \brief Read data from the serial port and put it into the data buffer.
	\details This function reads up to \a maxLength bytes from the port (non-blocking) and
	puts it into the \a data buffer.
	\param maxLength The maximum amount of data read.
	\param data The buffer that will store the received data.
	\returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be
			returned. Check data.size() for the number of bytes that were read.
*/
XsResultValue SerialInterface::readData(XsSize maxLength, XsByteArray& data)
{
	if (!isOpen())
		return (m_lastResult = XRV_NOPORTOPEN);

#ifdef _WIN32
	DWORD length;
	data.setSize(maxLength);
	BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL);
	data.pop_back(maxLength-length);
	JLTRACE(gJournal, "ReadFile result " << rres << ", length " << length);

	if (!rres)
	{
		DWORD wErr = ::GetLastError();
		JLALERT(gJournal, "ReadFile returned windows error " << wErr);
		if (wErr >= ERROR_INVALID_FUNCTION && wErr <= ERROR_INVALID_HANDLE)
			return (m_lastResult = XRV_NOFILEORPORTOPEN);
		return (m_lastResult = XRV_ERROR);
	}

	if (length == 0)
		return (m_lastResult = XRV_TIMEOUT);
#else
	fd_set fd;
	fd_set err;
	timeval timeout;
	FD_ZERO(&fd);
	FD_ZERO(&err);
	FD_SET(m_handle, &fd);
	FD_SET(m_handle, &err);

	timeout.tv_sec = m_timeout/1000;
	timeout.tv_usec = (m_timeout - (timeout.tv_sec * 1000)) * 1000;

	int res = select(FD_SETSIZE, &fd, NULL, &err, &timeout);
	if (res < 0 || FD_ISSET(m_handle, &err))
	{
		data.clear();
		return (m_lastResult = XRV_ERROR);
	} else if (res == 0) {
		data.clear();
		return (m_lastResult = XRV_TIMEOUT);
	}

	data.setSize(maxLength);
	int length = read(m_handle, (void*)data.data(), maxLength);
	data.pop_back(maxLength - length);
#endif

#ifdef LOG_RX_TX
	if (length > 0)
	{
		if (rx_log == NULL)
		{
			char fname[XS_MAX_FILENAME_LENGTH];
#ifdef _WIN32
			sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate);
#else
			char *devname = strrchr(m_portname, '/');
			sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate));
#endif
			makeFilenameUnique(fname);
			
			rx_log = fopen(fname, "wb");
		}
		fwrite(data.data(), 1, length, rx_log);
#ifdef LOG_RX_TX_FLUSH
		fflush(rx_log);
#endif
	}
#endif

	JLTRACE(gJournal, "returned success, read " << length << " of " << maxLength << " bytes, first: " << JLHEXLOG(data[0]));
	return (m_lastResult = XRV_OK);
}
コード例 #3
0
ファイル: serialinterface.cpp プロジェクト: Aharobot/mrpt
/*! \brief Read data from the serial port and put it into the data buffer.
	\details This function reads up to \a maxLength bytes from the port (non-blocking) and
	puts it into the \a data buffer.
	\param maxLength The maximum amount of data read.
	\param data The buffer that will store the received data.
	\returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be
			returned. Check data.size() for the number of bytes that were read.
*/
XsResultValue SerialInterface::readData(XsSize maxLength, XsByteArray& data)
{
	if (!isOpen())
		return (m_lastResult = XRV_NOPORTOPEN);

#ifdef _WIN32
	DWORD length;
	data.setSize(maxLength);
	BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL);
	data.pop_back(maxLength-length);
	JLTRACE(gJournal, "ReadFile result " << rres << ", length " << length);

	if (!rres)
	{
		JLALERT(gJournal, "ReadFile returned windows error " << ::GetLastError());
		return (m_lastResult = XRV_ERROR);
	}

	if (length == 0)
		return (m_lastResult = XRV_TIMEOUT);
#else
	fd_set fd;
	fd_set err;
	timeval timeout;
	FD_ZERO(&fd);
	FD_ZERO(&err);
	FD_SET(m_handle, &fd);
	FD_SET(m_handle, &err);

	timeout.tv_sec = m_timeout/1000;
	timeout.tv_usec = (m_timeout - (timeout.tv_sec * 1000)) * 1000;

	int res = select(FD_SETSIZE, &fd, NULL, &err, &timeout);
	if (res < 0 || FD_ISSET(m_handle, &err))
	{
		data.clear();
		return (m_lastResult = XRV_ERROR);
	} else if (res == 0) {
		data.clear();
		return (m_lastResult = XRV_TIMEOUT);
	}

	data.setSize(maxLength);
	int length = read(m_handle, (void*)data.data(), maxLength);
	data.pop_back(maxLength - length);
//	if (m_callbackHandler != NULL && *length > 0) {
//		XsBinary bytes;
//		bytes.setPortNumber(m_port);
//		bytes.setData(data, *length);
//#ifdef LOG_CALLBACKS
//		JLDEBUG(gJournal, "XsensDeviceAPI", "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_callbackHandler->onBytesReceived(bytes);
//	}
#endif

#ifdef LOG_RX_TX
	if (length > 0)
	{
		if (rx_log == NULL)
		{
			char fname[XS_MAX_FILENAME_LENGTH];
#ifdef _WIN32
			sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate);
#else
			char *devname = strrchr(m_portname, '/');
			sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate));
#endif
			rx_log = fopen(fname, "wb");
		}
		fwrite(data.data(), 1, length, rx_log);
		fflush(rx_log);
	}
#endif

	JLTRACE(gJournal, "returned success, read " << length << " of " << maxLength << " bytes, first: " << JLHEXLOG(data[0]));
	return (m_lastResult = XRV_OK);
}
コード例 #4
0
/*! \brief Read data from the serial port and put it into the data buffer.
	\details This function reads up to \a maxLength bytes from the port (non-blocking) and
	puts it into the \a data buffer.
	\param maxLength The maximum amount of data read.
	\param data The buffer that will store the received data.
	\returns XRV_OK if no error occurred. It can be that no data is available and XRV_OK will be
			returned. Check data.size() for the number of bytes that were read.
*/
XsResultValue SerialInterface::readData(XsSize maxLength, XsByteArray& data)
{
	if (!isOpen())
		return (m_lastResult = XRV_NOPORTOPEN);

#ifdef _WIN32
	DWORD length;
	data.setSize(maxLength);
	BOOL rres = ::ReadFile(m_handle, data.data(), (DWORD) maxLength, &length, NULL);
	data.pop_back(maxLength-length);
	JLTRACE(gJournal, "ReadFile result " << rres << ", length " << length);

	if (!rres)
	{
		DWORD wErr = ::GetLastError();
		JLALERT(gJournal, "ReadFile returned windows error " << wErr);
		if (wErr == ERROR_ACCESS_DENIED)
			return (m_lastResult = XRV_UNEXPECTED_DISCONNECT);
		if (wErr >= ERROR_INVALID_FUNCTION && wErr <= ERROR_INVALID_HANDLE)
			return (m_lastResult = XRV_NOFILEORPORTOPEN);
		return (m_lastResult = XRV_ERROR);
	}

	if (length == 0)
		return (m_lastResult = XRV_TIMEOUT);
#else
	fd_set fd;
	fd_set err;
	timeval timeout;
	FD_ZERO(&fd);
	FD_ZERO(&err);
	FD_SET(m_handle, &fd);
	FD_SET(m_handle, &err);

	timeout.tv_sec = m_timeout/1000;
	timeout.tv_usec = (m_timeout - (timeout.tv_sec * 1000)) * 1000;

	int res = select(FD_SETSIZE, &fd, NULL, &err, &timeout);
	if (res < 0 || FD_ISSET(m_handle, &err))
	{
		data.clear();
		return (m_lastResult = XRV_ERROR);
	} else if (res == 0) {
		data.clear();
		return (m_lastResult = XRV_TIMEOUT);
	}

	data.setSize(maxLength);
	int length = read(m_handle, (void*)data.data(), maxLength);
	if (length > 0)
	{
		data.pop_back(maxLength - length);
	}
	else
	{
		int err = errno;

		data.clear();
		switch (err)
		{
		case EAGAIN:
#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN
		case EWOULDBLOCK:
#endif
			return XRV_TIMEOUT;

		case EIO:
			return XRV_UNEXPECTED_DISCONNECT;

		default:
			break;
		}
	}
#if defined(JLLOGLEVEL) && JLLOGLEVEL <= JLL_TRACE && !defined(ANDROID)
	serial_icounter_struct ic;
	res = ioctl(m_handle, TIOCGICOUNT, &ic);
	if (res == 0)
	{
		JLTRACE(gJournal, "rx: " << ic.rx);
		JLTRACE(gJournal, "tx: " << ic.tx);
		JLTRACE(gJournal, "frame " << ic.frame);
		JLTRACE(gJournal, "overrun " << ic.overrun);
		JLTRACE(gJournal, "buf_overrun " << ic.buf_overrun);
	}
#endif
#endif

#ifdef LOG_RX_TX
	if (length > 0)
	{
		if (!rx_log.isOpen())
		{
			char fname[XS_MAX_FILENAME_LENGTH];
#ifdef _WIN32
			sprintf(fname, "rx_%03d_%d.log", (int32_t) m_port, m_baudrate);
#else
			char *devname = strrchr(m_portname, '/');
			sprintf(fname, "rx_%s_%d.log", devname + 1, XsBaud::rateToNumeric(m_baudrate));
#endif
			makeFilenameUnique(fname);

			rx_log.create(XsString(fname), true);
		}
		rx_log.write(data.data(), 1, length);
#ifdef LOG_RX_TX_FLUSH
		rx_log.flush();
#endif
	}
#endif

	JLTRACE(gJournal, "returned success, read " << length << " of " << maxLength << " bytes, first: " << JLHEXLOG(data[0]));
	return (m_lastResult = XRV_OK);
}