C++ XsByteArray::data方法代码示例

/*! \copydoc IoInterface::writeData
	\note The default timeout is respected in this operation.
*/
XsResultValue SerialInterface::writeData (const XsByteArray& data, XsSize* written)
{
	XsSize bytes;
	if (written == NULL)
		written = &bytes;

	if (!isOpen())
		return (m_lastResult = XRV_NOPORTOPEN);

	*written = 0;

#ifdef _WIN32
	DWORD lwritten = 0;
	if (WriteFile(m_handle, data.data(), (DWORD) data.size(), &lwritten, NULL) == 0)
		return (m_lastResult = XRV_ERROR);

	*written = lwritten;
#else
	ssize_t result = write(m_handle, (const void*)data.data(), data.size());
	if (result < 0)
		return (m_lastResult = XRV_ERROR);

	*written = result;
#endif

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

	return (m_lastResult = XRV_OK);
}

/*! \brief Find a string of bytes in the file
	\details The function searches from the current read position until the given \c needle is
	found. If the needle is not found, XsResultValue::NOT_FOUND is returned. The function
	will update the seek position to the first character of the found needle.
	\param needleV	The byte string to find.
	\param pos	The position where \a needleV was found. This will point to the first character
				of the found \a needleV.
	\returns XRV_OK if the data was found, XRV_ENDOFFILE if it wasn't found
*/
XsResultValue IoInterfaceFile::find(const XsByteArray& needleV, XsFilePos& pos)
{
	if (!m_handle)
		return m_lastResult = XRV_NOFILEOPEN;

	XsSize needleLength = needleV.size();

	pos = 0;
	if (needleLength == 0)
		return m_lastResult = XRV_OK;

	const char* needle = (const char*) needleV.data();

	gotoRead();

	char buffer[512];
	uint32_t bufferPos, needlePos = 0;
	size_t readBytes;
	if (m_readPos & 0x1FF)										// read a block of data
		readBytes = fread(buffer, 1, (512-((size_t) m_readPos & 0x1FF)), m_handle);
	else
		readBytes = fread(buffer, 1, 512, m_handle);		// read a block of data

	while (readBytes > 0)
	{
		m_readPos += readBytes;
		bufferPos = 0;

		while (bufferPos < readBytes && needlePos < needleLength)
		{
			if (buffer[bufferPos] == needle[needlePos])
			{
				// found a byte
				++needlePos;
			}
			else
			{
				if (needlePos > 0)
					needlePos = 0;
				else
				if (buffer[bufferPos] == needle[0])
				{
					// found a byte
					needlePos = 1;
				}
			}
			++bufferPos;
		}
		if (needlePos < needleLength)
			readBytes = fread(buffer, 1, 512, m_handle);	// read next block
		else
		{
			m_readPos = m_readPos + bufferPos - readBytes - needleLength; // or without needleLength
			pos = m_readPos; // - needleLength;
			FSEEK(m_readPos);
			return m_lastResult = XRV_OK;
		}
	}
	return m_lastResult = XRV_ENDOFFILE;
}

/*! \brief Read data from the USB 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 UsbInterface::readData(XsSize maxLength, XsByteArray& data)
{
	XsSize length = 0;
	data.setSize(maxLength);
	XsResultValue res = readData(maxLength, data.data(), &length);
	data.pop_back(maxLength - length);
	return res;
}

/*! \brief Write data to the end of the file.
	\details The function writes the given data to the file at the end. The
   current write
	position is also moved to the end of the file.
	\param bdata The byte data to append to the file
	\returns XRV_OK if the write was successful
*/
XsResultValue IoInterfaceFile::appendData(const XsByteArray& bdata)
{
	if (!m_handle) return m_lastResult = XRV_NOFILEOPEN;
	if (m_readOnly) return m_lastResult = XRV_READONLY;
	if (!bdata.size()) return m_lastResult = XRV_OK;

	if (m_reading || m_writePos != m_fileSize)
	{
		m_reading = false;
		FSEEK_R(0);  // lint !e534
	}
	size_t bytesWritten = fwrite(bdata.data(), 1, bdata.size(), m_handle);
	(void)bytesWritten;
	m_writePos = FTELL();
	m_fileSize = m_writePos;

	return (m_lastResult = XRV_OK);
}

/*! \brief Insert the given data into the file.
	\details The function writes the given data to the file at the current write
   position. This
	operation may take a while to complete.

	The write position is placed at the end of the inserted data.
	\param start The offset in the file to write the first byte
	\param data The data to insert in the file
	\returns XRV_OK if the data was inserted successfully
*/
XsResultValue IoInterfaceFile::insertData(
	XsFilePos start, const XsByteArray& data)
{
	if (!m_handle) return m_lastResult = XRV_NOFILEOPEN;
	if (m_readOnly) return m_lastResult = XRV_READONLY;

	gotoWrite();

	XsSize length = data.size();
	XsFilePos rPos = start;
	XsFilePos wPos = rPos + length;

	size_t read1, read2;
	XsFilePos remaining = m_fileSize - start;
	size_t bsize = (length > 512) ? length : 512;
	char* bufferRoot = (char*)malloc(bsize * 2);
	if (!bufferRoot) return XRV_OUTOFMEMORY;
	char* buffer1 = bufferRoot;
	char* buffer2 = bufferRoot + bsize;
	char* btemp;

	// copy data
	FSEEK(rPos);

	if (data.size() == 0) return m_lastResult = XRV_OK;

	if (remaining >= (XsFilePos)bsize)
		read1 = fread(buffer1, 1, bsize, m_handle);
	else
		read1 = fread(buffer1, 1, (size_t)remaining, m_handle);

	remaining -= read1;
	rPos += read1;

	while (remaining > 0)
	{
		// move data to correct buffer
		read2 = read1;
		btemp = buffer1;
		buffer1 = buffer2;
		buffer2 = btemp;

		// read next block
		if (remaining >= (XsFilePos)bsize)
			read1 = fread(buffer1, 1, bsize, m_handle);
		else
			read1 = fread(buffer1, 1, (size_t)remaining, m_handle);

		remaining -= read1;
		rPos += read1;

		// write block to the correct position
		FSEEK(wPos);
		wPos += fwrite(buffer2, 1, read2, m_handle);
		FSEEK(rPos);
	}

	FSEEK(wPos);
	wPos += fwrite(buffer1, 1, read1, m_handle);

	FSEEK(start);
	m_writePos = start + fwrite(data.data(), 1, length, m_handle);
	m_fileSize += length;

	free(bufferRoot);
	return m_lastResult = XRV_OK;
}

/*! \copydoc IProtocolHandler::findMessage
	\todo Since the assumption is that we receive a stream of valid messages without garbage, the scan
	is implemented in a rather naive and simple way. If we can expect lots of garbage in the data
	stream, this should probably be looked into.
*/
MessageLocation ProtocolHandler::findMessage(XsMessage& rcv, const XsByteArray& raw) const
{
	JLTRACE(gJournal, "Entry");
	MessageLocation rv(-1,0);
	rcv.clear();

	int bufferSize = (int) raw.size();
	if (bufferSize == 0)
		return rv;
	
	const unsigned char* buffer = raw.data();

	// loop through the buffer to find a preamble
	for (int pre = 0; pre < bufferSize; ++pre)
	{
		if (buffer[pre] == XS_PREAMBLE)
		{
			JLTRACE(gJournal, "Preamble found at " << pre);
			// we found a preamble, see if we can read a message from here
			if (rv.m_startPos == -1)
				rv.m_startPos = (int32_t) pre;
			int remaining = bufferSize-pre;	// remaining bytes in buffer INCLUDING preamble

			if (remaining < XS_LEN_MSGHEADERCS)
			{
				JLTRACE(gJournal, "Not enough header data read");
				if (rv.m_startPos != pre)
					continue;
				rv.m_size = -expectedMessageSize(&buffer[pre], remaining);
				return rv;
			}

			// read header
			const uint8_t* msgStart = &(buffer[pre]);
			const XsMessageHeader* hdr = (const XsMessageHeader*) msgStart;
			if (hdr->m_length == XS_EXTLENCODE)
			{
				if (remaining < XS_LEN_MSGEXTHEADERCS)
				{
					JLTRACE(gJournal, "Not enough extended header data read");
					if (rv.m_startPos != pre)
						continue;
					rv.m_size = -expectedMessageSize(&buffer[pre], remaining);
					return rv;
				}
			}
			else if (hdr->m_busId == 0 && hdr->m_messageId == 0)
			{
				// found 'valid' message that isn't actually valid... happens inside GPS raw data
				// skip to next preamble
				continue;
			}

			// check the reported size
			int target = expectedMessageSize(&buffer[pre], remaining);

			JLTRACE(gJournal, "Bytes in buffer=" << remaining << ", full target = " << target);
			if (target > (XS_LEN_MSGEXTHEADERCS + XS_MAXDATALEN))
			{
				// skip current preamble
				JLALERT(gJournal, "Invalid message length: " << target);
				rv.m_startPos = -1;
				continue;
			}

			if (remaining < target)
			{
				// not enough data read, skip current preamble
				JLTRACE(gJournal, "Not enough data read: " << remaining << " / " << target);
				if (rv.m_size == 0)
					rv.m_size = -target;
				continue;
			}

			// we have read enough data to fulfill our target so we'll try to parse the message
			// and check the checksum
			//if (rcv->loadFromString(msgStart, (uint16_t) target) == XRV_OK)
			if (rcv.loadFromString(msgStart, (uint16_t)target))
			{
				JLTRACE(gJournal,
						"OK, size = " << (int) rcv.getTotalMessageSize()
						<< std::hex << std::setfill('0')
						<< " First bytes " << std::setw(2) << (int) msgStart[0]
						<< " " << std::setw(2) << (int) msgStart[1]
						<< " " << std::setw(2) << (int) msgStart[2]
						<< " " << std::setw(2) << (int) msgStart[3]
						<< " " << std::setw(2) << (int) msgStart[4]
						<< std::dec << std::setfill(' '));
				rv.m_size = (int) rcv.getTotalMessageSize();
				rv.m_startPos = pre;	// we do this again here because this may not be the first preamble encountered (the check for -1 at the start of the loop is necessary)
				return rv;
			}

			// we could not read the message, clear message and try next preamble
			rcv.clear();
//.........这里部分代码省略.........

/*! \brief Write the data to the USB port.
	\details The function writes the given data to the connected USB port.
	The default timeout is respected in this operation.
	\param data The data to be written
	\param written An optional pointer to storage for the actual number of bytes that were written
	\returns XRV_OK if the data was successfully written
	\sa writeData(const XsSize, const void *, XsSize*)
*/
XsResultValue UsbInterface::writeData(const XsByteArray& data, XsSize* written)
{
	return writeData(data.size(), data.data(), written);
}

/*! \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);
}

/*! \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);
}

/*! \copydoc IoInterface::writeData
	\note The default timeout is respected in this operation.
*/
XsResultValue SerialInterface::writeData (const XsByteArray& data, XsSize* written)
{
	XsSize bytes;
	if (written == NULL)
		written = &bytes;

	if (!isOpen())
		return (m_lastResult = XRV_NOPORTOPEN);

	*written = 0;

#ifdef _WIN32
	DWORD lwritten = 0;
	if (WriteFile(m_handle, data.data(), (DWORD) data.size(), &lwritten, NULL) == 0)
	{
		DWORD wErr = ::GetLastError();
		JLALERT(gJournal, "WriteFile returned windows error " << wErr);
		if (wErr == ERROR_ACCESS_DENIED)
			return (m_lastResult = XRV_UNEXPECTED_DISCONNECT);
		return (m_lastResult = XRV_ERROR);
	}

	*written = lwritten;
#else
	ssize_t result = write(m_handle, (const void*)data.data(), data.size());
	if (result <= 0)
	{
		int err = errno;
		*written = 0;
		switch (err)
		{
		case EAGAIN:
#if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN
		case EWOULDBLOCK:
#endif
			return XRV_TIMEOUT;

		case EIO:
			return XRV_UNEXPECTED_DISCONNECT;

		/* we don't expect any other errors to actually occur */
		default:
			break;
		}
	}

	if (result < 0)
		*written = 0;
	else
		*written = result;
#endif

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

			tx_log.create(XsString(fname), true);
		}
		tx_log.write(data.data(), 1, *written);
#ifdef LOG_RX_TX_FLUSH
		tx_log.flush();
#endif
	}
#endif

	return (m_lastResult = XRV_OK);
}

/*! \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())
		{
//.........这里部分代码省略.........