text: 
  #include "vn/sensors.h"
  #include "vn/serialport.h"
  #include "vn/criticalsection.h"
  #include "vn/vntime.h"
  #include "vn/event.h"
  #include "vn/exceptions.h"
  #include "vn/error_detection.h"
  #include "vn/vector.h"
  #include "vn/matrix.h"
  #include "vn/compiler.h"
  #include "vn/util.h"
  #include "vn/thread.h"
  
  
  #include <string>
  #include <queue>
  #include <string.h>
  #include <stdio.h>
  
  #if PYTHON
  	#include "util.h"
  #endif
  
  using namespace std;
  using namespace vn::math;
  using namespace vn::xplat;
  using namespace vn::protocol::uart;
  using namespace vn::data::integrity;
  
  #define COMMAND_MAX_LENGTH 0x100
  
  namespace vn {
  namespace sensors {
  
  sensor_error::sensor_error()
  { }
  
  sensor_error::sensor_error(SensorError e) :
  	exception(),
  	error(e),
  	_errorMessage(NULL)
  { }
  
  sensor_error::sensor_error(sensor_error const& e) :
  	exception(),
  	error(e.error),
  	_errorMessage(NULL)
  { }
  
  sensor_error::~sensor_error() throw()
  {
  	if (_errorMessage != NULL)
  		delete[] _errorMessage;
  }
  
  char const* sensor_error::what() const throw()
  {
  	if (_errorMessage != NULL)
  		return _errorMessage;
  
  	string errorMsg = "received sensor error '" + str(error) + "'";
  
  	const_cast<char*&>(_errorMessage) = new char[errorMsg.size() + 1];
  
  	#if VN_HAVE_SECURE_CRT
  	strcpy_s(_errorMessage, errorMsg.size() + 1, errorMsg.c_str());
  	#else
  	strcpy(_errorMessage, errorMsg.c_str());
  	#endif
  
  	return _errorMessage;
  }
  
  struct VnSensor::Impl
  {
  	static const size_t DefaultReadBufferSize = 256;
  	static const uint16_t DefaultResponseTimeoutMs = 500;
  	static const uint16_t DefaultRetransmitDelayMs = 200;
  
  	SerialPort *pSerialPort;
  	char readBuffer[DefaultReadBufferSize];
  	IPort* port;
  	bool SimplePortIsOurs;
  	bool DidWeOpenSimplePort;
  	RawDataReceivedHandler _rawDataReceivedHandler;
  	void* _rawDataReceivedUserData;
  	PossiblePacketFoundHandler _possiblePacketFoundHandler;
  	void* _possiblePacketFoundUserData;
  	PacketFinder _packetFinder;
  	size_t _dataRunningIndex;
  	AsyncPacketReceivedHandler _asyncPacketReceivedHandler;
  	void* _asyncPacketReceivedUserData;
  	ErrorDetectionMode _sendErrorDetectionMode;
  	VnSensor* BackReference;
  	queue<Packet> _receivedResponses;
  	CriticalSection _transactionCS;
  	bool _waitingForResponse;
  	bool _filteringBootloaderResponses;
  	ErrorPacketReceivedHandler _errorPacketReceivedHandler;
  	void* _errorPacketReceivedUserData;
  	uint16_t _responseTimeoutMs;
  	uint16_t _retransmitDelayMs;
  	xplat::Event _newResponsesEvent;
  	#if PYTHON
  	PyObject* _rawDataReceivedHandlerPython;
  	PyObject* _asyncPacketReceivedHandlerPython;
  	#endif
  
  	explicit Impl(VnSensor* backReference) :
  		pSerialPort(NULL),
  		port(NULL),
  		SimplePortIsOurs(false),
  		DidWeOpenSimplePort(false),
  		_rawDataReceivedHandler(NULL),
  		_rawDataReceivedUserData(NULL),
  		_possiblePacketFoundHandler(NULL),
  		_possiblePacketFoundUserData(NULL),
  		_dataRunningIndex(0),
  		_asyncPacketReceivedHandler(NULL),
  		_asyncPacketReceivedUserData(NULL),
  		_sendErrorDetectionMode(ERRORDETECTIONMODE_CHECKSUM),
  		BackReference(backReference),
  		_waitingForResponse(false),
  		_filteringBootloaderResponses(false),
  		_errorPacketReceivedHandler(NULL),
  		_errorPacketReceivedUserData(NULL),
  		_responseTimeoutMs(DefaultResponseTimeoutMs),
  		_retransmitDelayMs(DefaultRetransmitDelayMs)
  		#if PYTHON
  		,
  		_asyncPacketReceivedHandlerPython(NULL),
  		_rawDataReceivedHandlerPython(NULL)
  		#endif
  	{
  		_packetFinder.registerPossiblePacketFoundHandler(this, possiblePacketFoundHandler);
  	}
  
  	~Impl()
  	{
          _packetFinder.unregisterPossiblePacketFoundHandler();
  	}
  
  	void onPossiblePacketFound(Packet& possiblePacket, size_t packetStartRunningIndex)
  	{
  		if (_possiblePacketFoundHandler != NULL)
  			_possiblePacketFoundHandler(_possiblePacketFoundUserData, possiblePacket, packetStartRunningIndex);
  	}
  
  	void onAsyncPacketReceived(Packet& asciiPacket, size_t runningIndex, TimeStamp timestamp)
  	{
  
  		if (_asyncPacketReceivedHandler != NULL)
  			_asyncPacketReceivedHandler(_asyncPacketReceivedUserData, asciiPacket, runningIndex);
  
  		#if PYTHON
  		BackReference->eventAsyncPacketReceived.fire(asciiPacket, runningIndex, timestamp);
  		#endif
  
  		//#if PYTHON
  		//if (_asyncPacketReceivedHandlerPython != NULL)
  		//{
  		//	python::AcquireGIL scopedLock;
  		//
  		//	boost::python::call<void>(_asyncPacketReceivedHandlerPython, boost::ref(asciiPacket), runningIndex);	
  		//}
  		//#endif
  	}
  
  	void onErrorPacketReceived(Packet& errorPacket, size_t runningIndex)
  	{
  		if (_errorPacketReceivedHandler != NULL)
  			_errorPacketReceivedHandler(_errorPacketReceivedUserData, errorPacket, runningIndex);
  	}
  
  	static void possiblePacketFoundHandler(void* userData, Packet& possiblePacket, size_t packetStartRunningIndex, TimeStamp timestamp)
  	{
  		Impl* pThis = static_cast<Impl*>(userData);
  
  		pThis->onPossiblePacketFound(possiblePacket, packetStartRunningIndex);
  
  		if (!possiblePacket.isValid())
  		{
  			return;
  		}
  
  		if (pThis->_filteringBootloaderResponses && !possiblePacket.isBootloader())
  		{
  			// If anything other than bootloader packets arrive, turn off the bootloader filter
  			pThis->_filteringBootloaderResponses = false;
  		}
  
  		if (possiblePacket.isError())
  		{
  			if (pThis->_waitingForResponse)
  			{
  				pThis->_transactionCS.enter();
  				pThis->_receivedResponses.push(possiblePacket);
  				pThis->_newResponsesEvent.signal();
  				pThis->_transactionCS.leave();
  			}
  
  			pThis->onErrorPacketReceived(possiblePacket, packetStartRunningIndex);
  
  			return;
  		}
  
  		if (possiblePacket.isResponse())
  		{
  			if (pThis->_waitingForResponse)
  			{
  				pThis->_transactionCS.enter();
  				pThis->_receivedResponses.push(possiblePacket);
  				pThis->_newResponsesEvent.signal();
  				pThis->_transactionCS.leave();
  			}
  
  			return;
  		}
  
  		// This wasn't anything else. We assume it is an async packet.
  		pThis->onAsyncPacketReceived(possiblePacket, packetStartRunningIndex, timestamp);
  	}
  
  	static void dataReceivedHandler(void* userData)
  	{
  		Impl *pi = static_cast<Impl*>(userData);
  
  		size_t numOfBytesRead = 0;
  
  		pi->port->read(
  			pi->readBuffer,
  			DefaultReadBufferSize,
  			numOfBytesRead);
  
  		if (numOfBytesRead == 0)
  			return;
  
  		TimeStamp t = TimeStamp::get();
  
  		if (pi->_rawDataReceivedHandler != NULL)
  			pi->_rawDataReceivedHandler(pi->_rawDataReceivedUserData, reinterpret_cast<char*>(pi->readBuffer), numOfBytesRead, pi->_dataRunningIndex);
  
  		#if PYTHON
  		if (pi->_rawDataReceivedHandlerPython != NULL)
  		{
  			vector<char> pRawData(readBuffer, readBuffer + numOfBytesRead);
  
  			python::AcquireGIL scopedLock;
  				
  			boost::python::call<void>(pi->_rawDataReceivedHandlerPython, pRawData, pi->_dataRunningIndex);
  		}
  		#endif
  
  		pi->_packetFinder.processReceivedData(reinterpret_cast<char*>(pi->readBuffer), numOfBytesRead, pi->_filteringBootloaderResponses, t);
  
  		pi->_dataRunningIndex += numOfBytesRead;
  	}
  
  	bool isConnected()
  	{
  		return port != NULL && port->isOpen();
  	}
  
  	size_t finalizeCommandToSend(char *toSend, size_t length)
  	{
  		#if defined(_MSC_VER)
  			// Unable to use save version 'sprintf_s' since the length of 'toSend' is unknown here.
  			#pragma warning(push)
  			#pragma warning(disable:4996)
  		#endif
  		
  		if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CHECKSUM)
  		{
  			length += sprintf(toSend + length, "%02X\r\n", Checksum8::compute(toSend + 1, length - 2));
  		}
  		else if (_sendErrorDetectionMode == ERRORDETECTIONMODE_CRC)
  		{
  			length += sprintf(toSend + length, "%04X\r\n", Crc16::compute(toSend + 1, length - 2));
  		}
  		else
  		{
  			length += sprintf(toSend + length, "XX\r\n");
  		}
  
  		#if defined(_MSC_VER)
  			#pragma warning(pop)
  		#endif
  
  		return length;
  	}
  
  	Packet transactionWithWait(char* toSend, size_t length, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs)
  	{
  		// Make sure we don't have any existing responses.
  		_transactionCS.enter();
  
  		#if VN_SUPPORTS_SWAP
  		queue<Packet> empty;
  		_receivedResponses.swap(empty);
  		#else
  		while (!_receivedResponses.empty()) _receivedResponses.pop();
  		#endif
  		_waitingForResponse = true;
  		_transactionCS.leave();
  
  		// Send the command and continue sending if retransmits are enabled
  		// until we receive the response or timeout.
  		Stopwatch timeoutSw;
  
  		port->write(toSend, length);
  		float curElapsedTime = timeoutSw.elapsedMs();
  
  		while (true)
  		{
  			bool shouldRetransmit = false;
  
  			// Compute how long we should wait for a response before taking
  			// more action.
  			float responseWaitTime = responseTimeoutMs - curElapsedTime;
  			if (responseWaitTime > retransmitDelayMs)
  			{
  				responseWaitTime = retransmitDelayMs;
  				shouldRetransmit = true;
  			}
  
  			// See if we have time left.
  			if (responseWaitTime < 0)
  			{
  				_waitingForResponse = false;
  				throw timeout();
  			}
  
  			// Wait for any new responses that come in or until it is time to
  			// send a new retransmit.
  			xplat::Event::WaitResult waitResult = _newResponsesEvent.waitUs(static_cast<uint32_t>(responseWaitTime * 1000));
  
  			queue<Packet> responsesToProcess;
  
  			if (waitResult == xplat::Event::WAIT_TIMEDOUT)
  			{
  				if (!shouldRetransmit)
  				{
  					_waitingForResponse = false;
  					throw timeout();
  				}
  			}
  
  			if (waitResult == xplat::Event::WAIT_SIGNALED)
  			{
  				// Get the current collection of responses.
  				_transactionCS.enter();
  				#if VN_SUPPORTS_SWAP
  				_receivedResponses.swap(responsesToProcess);
  				#else
  				while (!_receivedResponses.empty())
  				{
  					responsesToProcess.push(_receivedResponses.front());
  					_receivedResponses.pop();
  				}
  				#endif
  				_transactionCS.leave();
  			}
  
  			// Process the collection of responses we have.
  			while (!responsesToProcess.empty())
  			{
  				Packet p = responsesToProcess.front();
  				responsesToProcess.pop();
  
  				if (p.isError())
  				{
  					_waitingForResponse = false;
  					throw sensor_error(p.parseError());
  				}
  
  				// We must have a response packet.
  				_waitingForResponse = false;
  				Thread::sleepMs(1);
  				return p;
  			}
  
  			// Retransmit.
  			port->write(toSend, length);
  			curElapsedTime = timeoutSw.elapsedMs();
  		}
  	}
  
  	void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs)
  	{
  		if (!isConnected())
  			throw invalid_operation();
  
  		if (waitForReply)
  		{
  			*response = transactionWithWait(toSend, length, responseTimeoutMs, retransmitDelayMs);
  
  			if (response->isError())
  				throw sensor_error(response->parseError());
  		}
  		else
  		{
  			port->write(toSend, length);
  		}
  	}
  
  	void transactionNoFinalize(char* toSend, size_t length, bool waitForReply, Packet *response)
  	{
  		transactionNoFinalize(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs);
  	}
  
  	void transaction(char* toSend, size_t length, bool waitForReply, Packet *response, uint16_t responseTimeoutMs, uint16_t retransmitDelayMs)
  	{
  		length = finalizeCommandToSend(toSend, length);
  
  		transactionNoFinalize(toSend, length, waitForReply, response, responseTimeoutMs, retransmitDelayMs);
  	}
  
  	// Performs a communication transaction with the sensor. If waitForReply is
  	// set to true, we will retransmit the message until we receive a response
  	// or until we time out, depending on current settings.
  	void transaction(char* toSend, size_t length, bool waitForReply, Packet *response)
  	{
  		transaction(toSend, length, waitForReply, response, _responseTimeoutMs, _retransmitDelayMs);
  	}
  
  	BinaryOutputRegister readBinaryOutput(uint8_t binaryOutputNumber)
  	{
  		char toSend[17];
  		Packet response;
  		uint16_t asyncMode, rateDivisor, outputGroup, commonField, timeField, imuField, gpsField, attitudeField, insField, gps2Field;
  
  		#if VN_HAVE_SECURE_CRT
  		int length = sprintf_s(toSend, sizeof(toSend), "$VNRRG,%u*", 74 + binaryOutputNumber);
  		#else
  		int length = sprintf(toSend, "$VNRRG,%u*", 74 + binaryOutputNumber);
  		#endif
  
  		transaction(toSend, length, true, &response);
  
  		response.parseBinaryOutput(
  			&asyncMode,
  			&rateDivisor,
  			&outputGroup,
  			&commonField,
  			&timeField,
  			&imuField,
  			&gpsField,
  			&attitudeField,
  			&insField,
        &gps2Field);
  
  		return BinaryOutputRegister(
  			static_cast<AsyncMode>(asyncMode),
  			rateDivisor,
  			static_cast<CommonGroup>(commonField),
  			static_cast<TimeGroup>(timeField),
  			static_cast<ImuGroup>(imuField),
  			static_cast<GpsGroup>(gpsField),
  			static_cast<AttitudeGroup>(attitudeField),
  			static_cast<InsGroup>(insField),
        static_cast<GpsGroup>(gps2Field));
  	}
  
  	void writeBinaryOutput(uint8_t binaryOutputNumber, BinaryOutputRegister &fields, bool waitForReply)
  	{
  		char toSend[256];
  		Packet response;
  
  		// First determine which groups are present.
  		uint16_t groups = 0;
  		if (fields.commonField)
  			groups |= 0x0001;
  		if (fields.timeField)
  			groups |= 0x0002;
  		if (fields.imuField)
  			groups |= 0x0004;
  		if (fields.gpsField)
  			groups |= 0x0008;
  		if (fields.attitudeField)
  			groups |= 0x0010;
  		if (fields.insField)
  			groups |= 0x0020;
      if(fields.gps2Field)
        groups |= 0x0040;
  
  		#if VN_HAVE_SECURE_CRT
  		int length = sprintf_s(toSend, sizeof(toSend), "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups);
  		#else
  		int length = sprintf(toSend, "$VNWRG,%u,%u,%u,%X", 74 + binaryOutputNumber, fields.asyncMode, fields.rateDivisor, groups);
  		#endif
  
  		if (fields.commonField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.commonField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.commonField);
  			#endif
  		if (fields.timeField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.timeField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.timeField);
  			#endif
  		if (fields.imuField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.imuField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.imuField);
  			#endif
  		if (fields.gpsField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gpsField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.gpsField);
  			#endif
  		if (fields.attitudeField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.attitudeField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.attitudeField);
  			#endif
  		if (fields.insField)
  			#if VN_HAVE_SECURE_CRT
  			length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.insField);
  			#else
  			length += sprintf(toSend + length, ",%X", fields.insField);
  			#endif
      if(fields.gps2Field)
        #if VN_HAVE_SECURE_CRT
        length += sprintf_s(toSend + length, sizeof(toSend) - length, ",%X", fields.gps2Field);
        #else
        length += sprintf(toSend + length, ",%X", fields.gps2Field);
        #endif
  
  		#if VN_HAVE_SECURE_CRT
  		length += sprintf_s(toSend + length, sizeof(toSend) - length, "*");
  		#else
  		length += sprintf(toSend + length, "*");
  		#endif
  
  		transaction(toSend, length, waitForReply, &response);
  	}
  };
  
  vector<uint32_t> VnSensor::supportedBaudrates()
  {
  	uint32_t br[] = {
  		9600,
  		19200,
  		38400,
  		57600,
  		115200,
  		128000,
  		230400,
  		460800,
  		921600 };
  
  	return vector<uint32_t>(br, br + sizeof(br) / sizeof(uint32_t));
  }
  
  #if defined(_MSC_VER) && _MSC_VER <= 1600
  	#pragma warning(push)
  	// Disable VS2010 warning for 'this' used in base member initializer list.
  	#pragma warning(disable:4355)
  #endif
  
  VnSensor::VnSensor() :
  	_pi(new Impl(this))
  { 
  	firmwareUpdateFile = NULL;
  	bootloaderFiltering = false;
  }
  
  #if defined(_MSC_VER) && _MSC_VER <= 1600
  	#pragma warning(pop)
  #endif
  
  VnSensor::~VnSensor()
  {
  	if (_pi != NULL)
  	{
  		if (_pi->SimplePortIsOurs && _pi->DidWeOpenSimplePort && isConnected())
  			disconnect();
  
  		delete _pi;
  		_pi = NULL;
  	}
  }
  
  uint32_t VnSensor::baudrate()
  {
  	if (_pi->pSerialPort == NULL)
  		// We are not connected to a known serial port.
  		throw invalid_operation();
  
  	return _pi->pSerialPort->baudrate();
  }
  
  std::string VnSensor::port()
  {
  	if (_pi->pSerialPort == NULL)
  		// We are not connected to a known serial port.
  		throw invalid_operation();
  
  	return _pi->pSerialPort->port();
  }
  
  ErrorDetectionMode VnSensor::sendErrorDetectionMode()
  {
  	return _pi->_sendErrorDetectionMode;
  }
  
  void VnSensor::setSendErrorDetectionMode(ErrorDetectionMode mode)
  {
  	_pi->_sendErrorDetectionMode = mode;
  }
  
  bool VnSensor::isConnected()
  {
  	return _pi->isConnected();
  }
  
  uint16_t VnSensor::responseTimeoutMs()
  {
  	return _pi->_responseTimeoutMs;
  }
  
  void VnSensor::setResponseTimeoutMs(uint16_t timeout)
  {
  	_pi->_responseTimeoutMs = timeout;
  }
  
  uint16_t VnSensor::retransmitDelayMs()
  {
  	return _pi->_retransmitDelayMs;
  }
  
  void VnSensor::setRetransmitDelayMs(uint16_t delay)
  {
  	_pi->_retransmitDelayMs = delay;
  }
  
  bool VnSensor::verifySensorConnectivity()
  {
  	try
  	{
  		readModelNumber();
  
  		return true;
  	}
  	catch (...) {	}
  
  	return false;
  }
  
  void VnSensor::connect(const string &portName, uint32_t baudrate)
  {
  	_pi->pSerialPort = new SerialPort(portName, baudrate);
  
  	connect(dynamic_cast<IPort*>(_pi->pSerialPort));
  }
  
  void VnSensor::connect(IPort* simplePort)
  {
  	_pi->port = simplePort;
  	_pi->SimplePortIsOurs = false;
  
  	_pi->port->registerDataReceivedHandler(_pi, Impl::dataReceivedHandler);
  
  	if (!_pi->port->isOpen())
  	{
  		_pi->port->open();
  		_pi->DidWeOpenSimplePort = true;
  	}
  }
  
  void VnSensor::disconnect()
  {
  	if (_pi->port == NULL || !_pi->port->isOpen())
  		throw invalid_operation();
  
  	_pi->port->unregisterDataReceivedHandler();
  
  	if (_pi->DidWeOpenSimplePort)
  	{
  		_pi->port->close();
  	}
  
  	_pi->DidWeOpenSimplePort = false;
  
  	if (_pi->SimplePortIsOurs)
  	{
  		delete _pi->port;
  
  		_pi->port = NULL;
  	}
  
  	if (_pi->pSerialPort != NULL)
  	{
  		// Assuming we created this serial port.
  		delete _pi->pSerialPort;
  		_pi->pSerialPort = NULL;
  	}
  }
  
  string VnSensor::transaction(string toSend)
  {
  	char buffer[COMMAND_MAX_LENGTH];
  	size_t finalLength = toSend.length();
  	Packet response;
  
  	// Copy over what was provided.
  	copy(toSend.begin(), toSend.end(), buffer);
  
  	// First see if an '*' is present.
  	if (toSend.find('*') == string::npos)
  	{
  		buffer[toSend.length()] = '*';
  		finalLength = _pi->finalizeCommandToSend(buffer, toSend.length() + 1);
  	}
  	else if (toSend[toSend.length() - 2] != '\r' && toSend[toSend.length() - 1] != '\n')
  	{
  		buffer[toSend.length()] = '\r';
  		buffer[toSend.length() + 1] = '\n';
  		finalLength += 2;
  	}
  
  	_pi->transactionNoFinalize(buffer, finalLength, true, &response);
  
  	return response.datastr();
  }
  
  string VnSensor::send(string toSend, bool waitForReply, ErrorDetectionMode errorDetectionMode)
  {
  	Packet p;
  	char *buffer = new char[toSend.size() + 8];	// Extra room for possible additions.
  	size_t curToSendLength = toSend.size();
  
  	// See if a '$' needs to be prepended.
  	if (toSend[0] == '$')
  	{
  		#if VN_HAVE_SECURE_SCL
  		toSend._Copy_s(buffer, toSend.size() + 8, toSend.size());
  		#else
  		toSend.copy(buffer, toSend.size());
  		#endif
  
  	}
  	else
  	{
  		buffer[0] = '$';
  		#if VN_HAVE_SECURE_SCL
  		toSend._Copy_s(buffer + 1, toSend.size() + 8 - 1, toSend.size());
  		#else
  		toSend.copy(buffer + 1, toSend.size());
  		#endif
  		curToSendLength++;
  	}
  
  	// Locate any '*' in the command.
  	size_t astrickLocation = string::npos;
  	for (size_t i = 0; i < curToSendLength; i++)
  	{
  		if (buffer[i] == '*')
  		{
  			astrickLocation = i;
  			break;
  		}
  	}
  
  	// Do we need to add a '*'?
  	if (astrickLocation == string::npos)
  	{
  		buffer[curToSendLength] = '*';
  		astrickLocation = curToSendLength++;
  	}
  
  	// Do we need to add a checksum/CRC?
  	if (astrickLocation == curToSendLength - 1)
  	{
  		if (errorDetectionMode == ERRORDETECTIONMODE_CHECKSUM)
  		{
  			#if VN_HAVE_SECURE_CRT
  			curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2));
  			#else
  			curToSendLength += sprintf(buffer + curToSendLength, "%02X\r\n", Checksum8::compute(buffer + 1, curToSendLength - 2));
  			#endif
  		}
  		else if (errorDetectionMode == ERRORDETECTIONMODE_CRC)
  		{
  			#if VN_HAVE_SECURE_CRT
  			curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2));
  			#else
  			curToSendLength += sprintf(buffer + curToSendLength, "%04X\r\n", Crc16::compute(buffer + 1, curToSendLength - 2));
  			#endif
  		}
  		else
  		{
  			#if VN_HAVE_SECURE_CRT
  			curToSendLength += sprintf_s(buffer + curToSendLength, toSend.size() + 8 - curToSendLength, "*XX\r\n");
  			#else
  			curToSendLength += sprintf(buffer + curToSendLength, "*XX\r\n");
  			#endif
  		}
  	}
  	// Do we need to add "\r\n"?
  	else if (buffer[curToSendLength - 1] != '\n')
  	{
  		buffer[curToSendLength++] = '\r';
  		buffer[curToSendLength++] = '\n';
  	}
  
  	_pi->transactionNoFinalize(buffer, curToSendLength, waitForReply, &p, _pi->_responseTimeoutMs, _pi->_retransmitDelayMs);
  
  	delete [] buffer;
  
  	return p.datastr();
  }
  
  void VnSensor::registerRawDataReceivedHandler(void* userData, RawDataReceivedHandler handler)
  {
  	if (_pi->_rawDataReceivedHandler != NULL)
  		throw invalid_operation();
  
  	_pi->_rawDataReceivedHandler = handler;
  	_pi->_rawDataReceivedUserData = userData;
  }
  
  #if PL150
  #else
  
  #if PYTHON
  
  PyObject* VnSensor::registerRawDataReceivedHandler(PyObject* callable)
  {
  	_pi->_rawDataReceivedHandlerPython = callable;
  	callable->ob_refcnt++;
  
  	return Py_None;
  }
  
  #endif
  #endif
  
  void VnSensor::unregisterRawDataReceivedHandler()
  {
  	if (_pi->_rawDataReceivedHandler == NULL)
  		throw invalid_operation();
  
  	_pi->_rawDataReceivedHandler = NULL;
  	_pi->_rawDataReceivedUserData = NULL;
  }
  
  void VnSensor::registerPossiblePacketFoundHandler(void* userData, PossiblePacketFoundHandler handler)
  {
  	if (_pi->_possiblePacketFoundHandler != NULL)
  		throw invalid_operation();
  
  	_pi->_possiblePacketFoundHandler = handler;
  	_pi->_possiblePacketFoundUserData = userData;
  }
  
  void VnSensor::unregisterPossiblePacketFoundHandler()
  {
  	if (_pi->_possiblePacketFoundHandler == NULL)
  		throw invalid_operation();
  
  	_pi->_possiblePacketFoundHandler = NULL;
  	_pi->_possiblePacketFoundUserData = NULL;
  }
  
  void VnSensor::registerAsyncPacketReceivedHandler(void* userData, AsyncPacketReceivedHandler handler)
  {
  	if (_pi->_asyncPacketReceivedHandler != NULL)
  		throw invalid_operation();
  
  	_pi->_asyncPacketReceivedHandler = handler;
  	_pi->_asyncPacketReceivedUserData = userData;
  }
  
  #if PL150
  
  #else
  #if PYTHON
  
  PyObject* VnSensor::registerAsyncPacketReceivedHandler(PyObject* callable)
  {
  	_pi->_asyncPacketReceivedHandlerPython = callable;
  	callable->ob_refcnt++;
  
  	return Py_None;
  }
  
  #endif
  #endif
  
  void VnSensor::unregisterAsyncPacketReceivedHandler()
  {
  	if (_pi->_asyncPacketReceivedHandler == NULL)
  		throw invalid_operation();
  
  	_pi->_asyncPacketReceivedHandler = NULL;
  	_pi->_asyncPacketReceivedUserData = NULL;
  }
  
  void VnSensor::registerErrorPacketReceivedHandler(void* userData, ErrorPacketReceivedHandler handler)
  {
  	if (_pi->_errorPacketReceivedHandler != NULL)
  		throw invalid_operation();
  
  	_pi->_errorPacketReceivedHandler = handler;
  	_pi->_errorPacketReceivedUserData = userData;
  }
  
  void VnSensor::unregisterErrorPacketReceivedHandler()
  {
  	if (_pi->_errorPacketReceivedHandler == NULL)
  		throw invalid_operation();
  
  	_pi->_errorPacketReceivedHandler = NULL;
  	_pi->_errorPacketReceivedUserData = NULL;
  }
  
  void VnSensor::writeSettings(bool waitForReply)
  {
  	char toSend[37];
  
  	size_t length = Packet::genWriteSettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  
  	// Write settings sometimes takes a while to do and receive a response
  	// from the sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000);
  }
  
  void VnSensor::tare(bool waitForReply)
  {
  	char toSend[14];
  
  	size_t length = Packet::genTare(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::setGyroBias(bool waitForReply)
  {
  	char toSend[14];
  
  	size_t length = Packet::genSetGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::magneticDisturbancePresent(bool disturbancePresent, bool waitForReply)
  {
  	char toSend[16];
  
  	size_t length = Packet::genKnownMagneticDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent);
  
  	Packet response;
  
  	// Write settings sometimes takes a while to do and receive a response
  	// from the sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::accelerationDisturbancePresent(bool disturbancePresent, bool waitForReply)
  {
  	char toSend[16];
  
  	size_t length = Packet::genKnownAccelerationDisturbance(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), disturbancePresent);
  
  	Packet response;
  
  	// Write settings sometimes takes a while to do and receive a response
  	// from the sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::restoreFactorySettings(bool waitForReply)
  {
  	char toSend[37];
  
  	size_t length = Packet::genRestoreFactorySettings(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  
  	// Restore factory settings sometimes takes a while to do and receive a
  	// response from the sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000);
  }
  
  void VnSensor::reset(bool waitForReply)
  {
  	char toSend[37];
  
  	size_t length = Packet::genReset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  
  	// Reset sometimes takes a while to do and receive a response from the
  	// sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response, 2500, 1000);
  }
  
  void VnSensor::firmwareUpdateMode(bool waitForReply)
  {
  	char toSend[37];
  
  	size_t length = Packet::genFirmwareUpdate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  
  	// Reset sometimes takes a while to do and receive a response from the sensor.
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response, 3000, 3000);
  }
  
  
  void VnSensor::changeBaudRate(uint32_t baudrate)
  {
      writeSerialBaudRate(baudrate, true);
  
  	_pi->pSerialPort->changeBaudrate(baudrate);
  }
  
  VnSensor::Family VnSensor::determineDeviceFamily()
  {
  	string mn = readModelNumber();
  
  	return determineDeviceFamily(mn);
  }
  
  VnSensor::Family VnSensor::determineDeviceFamily(std::string modelNumber)
  {
  	if (modelNumber.find("VN-100") == 0)
  		return VnSensor_Family_Vn100;
  
  	if (modelNumber.find("VN-200") == 0)
  		return VnSensor_Family_Vn200;
  
  	if (modelNumber.find("VN-300") == 0)
  		return VnSensor_Family_Vn300;
  
  	return VnSensor_Family_Unknown;
  }
  
  BinaryOutputRegister VnSensor::readBinaryOutput1()
  {
  	return _pi->readBinaryOutput(1);
  }
  
  BinaryOutputRegister VnSensor::readBinaryOutput2()
  {
  	return _pi->readBinaryOutput(2);
  }
  
  BinaryOutputRegister VnSensor::readBinaryOutput3()
  {
  	return _pi->readBinaryOutput(3);
  }
  
  
  void VnSensor::writeBinaryOutput1(BinaryOutputRegister &fields, bool waitForReply)
  {
  	_pi->writeBinaryOutput(1, fields, waitForReply);
  }
  
  void VnSensor::writeBinaryOutput2(BinaryOutputRegister &fields, bool waitForReply)
  {
  	_pi->writeBinaryOutput(2, fields, waitForReply);
  }
  
  void VnSensor::writeBinaryOutput3(BinaryOutputRegister &fields, bool waitForReply)
  {
  	_pi->writeBinaryOutput(3, fields, waitForReply);
  }
  
  
  uint32_t VnSensor::readSerialBaudRate(uint8_t port)
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseSerialBaudRate(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, uint8_t port, bool waitForReply)
  {
  	char toSend[25];
  
  	size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate, port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  AsciiAsync VnSensor::readAsyncDataOutputType(uint8_t port)
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseAsyncDataOutputType(&reg);
  
  	return static_cast<AsciiAsync>(reg);
  }
  
  void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, uint8_t port, bool waitForReply)
  {
  	char toSend[21];
  
  	size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador, port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  uint32_t VnSensor::readAsyncDataOutputFrequency(uint8_t port)
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseAsyncDataOutputFrequency(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, uint8_t port, bool waitForReply)
  {
  	char toSend[26];
  
  	size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof, port);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  InsBasicConfigurationRegisterVn200 VnSensor::readInsBasicConfigurationVn200()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsBasicConfigurationRegisterVn200 reg;
  	uint8_t scenario;
  	uint8_t ahrsAiding;
  	response.parseInsBasicConfiguration(
  		&scenario,
  		&ahrsAiding);
  
  	reg.scenario = static_cast<Scenario>(scenario);
  	reg.ahrsAiding = ahrsAiding != 0;
  
  	return reg;
  }
  
  void VnSensor::writeInsBasicConfigurationVn200(InsBasicConfigurationRegisterVn200 &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, 0);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeInsBasicConfigurationVn200(
  	Scenario scenario,
  	const uint8_t &ahrsAiding,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsBasicConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		scenario,
  		ahrsAiding,
  		0);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  InsBasicConfigurationRegisterVn300 VnSensor::readInsBasicConfigurationVn300()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsBasicConfigurationRegisterVn300 reg;
  	uint8_t scenario;
  	uint8_t ahrsAiding;
  	uint8_t estBaseline;
  	response.parseInsBasicConfiguration(
  		&scenario,
  		&ahrsAiding,
  		&estBaseline);
  
  	reg.scenario = static_cast<Scenario>(scenario);
  	reg.ahrsAiding = ahrsAiding != 0;
  	reg.estBaseline = estBaseline != 0;
  
  	return reg;
  }
  
  void VnSensor::writeInsBasicConfigurationVn300(InsBasicConfigurationRegisterVn300 &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsBasicConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.scenario, fields.ahrsAiding, fields.estBaseline);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeInsBasicConfigurationVn300(
  	Scenario scenario,
  	const uint8_t &ahrsAiding,
  	const uint8_t &estBaseline,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsBasicConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		scenario,
  		ahrsAiding,
  		estBaseline);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  string VnSensor::readUserTag()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	char tagBuffer[50];
  	response.parseUserTag(tagBuffer);
  
  	return tagBuffer;
  }
  
  void VnSensor::writeUserTag(const string &tag, bool waitForReply)
  {
  	char toSend[37];
  
  	size_t length = Packet::genWriteUserTag(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), tag.c_str());
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  string VnSensor::readModelNumber()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadModelNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	char productNameBuffer[50];
  	response.parseModelNumber(productNameBuffer);
  
  	return productNameBuffer;
  }
  
  uint32_t VnSensor::readHardwareRevision()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadHardwareRevision(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseHardwareRevision(&reg);
  
  	return reg;
  }
  
  uint32_t VnSensor::readSerialNumber()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadSerialNumber(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseSerialNumber(&reg);
  
  	return reg;
  }
  
  string VnSensor::readFirmwareVersion()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadFirmwareVersion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	char firmwareVersionBuffer[50];
  	response.parseFirmwareVersion(firmwareVersionBuffer);
  
  	return firmwareVersionBuffer;
  }
  
  uint32_t VnSensor::readSerialBaudRate()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseSerialBaudRate(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeSerialBaudRate(const uint32_t &baudrate, bool waitForReply)
  {
  	char toSend[25];
  
  	size_t length = Packet::genWriteSerialBaudRate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), baudrate);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  AsciiAsync VnSensor::readAsyncDataOutputType()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseAsyncDataOutputType(&reg);
  
  	return static_cast<AsciiAsync>(reg);
  }
  
  void VnSensor::writeAsyncDataOutputType(AsciiAsync ador, bool waitForReply)
  {
  	char toSend[19];
  
  	size_t length = Packet::genWriteAsyncDataOutputType(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), ador);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  uint32_t VnSensor::readAsyncDataOutputFrequency()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	uint32_t reg;
  	response.parseAsyncDataOutputFrequency(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeAsyncDataOutputFrequency(const uint32_t &adof, bool waitForReply)
  {
  	char toSend[26];
  
  	size_t length = Packet::genWriteAsyncDataOutputFrequency(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), adof);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  vec3f VnSensor::readYawPitchRoll()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadYawPitchRoll(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseYawPitchRoll(&reg);
  
  	return reg;
  }
  
  vec4f VnSensor::readAttitudeQuaternion()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAttitudeQuaternion(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec4f reg;
  	response.parseAttitudeQuaternion(&reg);
  
  	return reg;
  }
  
  QuaternionMagneticAccelerationAndAngularRatesRegister VnSensor::readQuaternionMagneticAccelerationAndAngularRates()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadQuaternionMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	QuaternionMagneticAccelerationAndAngularRatesRegister reg;
  	response.parseQuaternionMagneticAccelerationAndAngularRates(
  		&reg.quat,
  		&reg.mag,
  		&reg.accel,
  		&reg.gyro);
  
  	return reg;
  }
  
  vec3f VnSensor::readMagneticMeasurements()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadMagneticMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseMagneticMeasurements(&reg);
  
  	return reg;
  }
  
  vec3f VnSensor::readAccelerationMeasurements()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAccelerationMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseAccelerationMeasurements(&reg);
  
  	return reg;
  }
  
  vec3f VnSensor::readAngularRateMeasurements()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAngularRateMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseAngularRateMeasurements(&reg);
  
  	return reg;
  }
  
  MagneticAccelerationAndAngularRatesRegister VnSensor::readMagneticAccelerationAndAngularRates()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	MagneticAccelerationAndAngularRatesRegister reg;
  	response.parseMagneticAccelerationAndAngularRates(
  		&reg.mag,
  		&reg.accel,
  		&reg.gyro);
  
  	return reg;
  }
  
  MagneticAndGravityReferenceVectorsRegister VnSensor::readMagneticAndGravityReferenceVectors()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	MagneticAndGravityReferenceVectorsRegister reg;
  	response.parseMagneticAndGravityReferenceVectors(
  		&reg.magRef,
  		&reg.accRef);
  
  	return reg;
  }
  
  void VnSensor::writeMagneticAndGravityReferenceVectors(MagneticAndGravityReferenceVectorsRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagneticAndGravityReferenceVectors(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magRef, fields.accRef);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeMagneticAndGravityReferenceVectors(
  	const vec3f &magRef,
  	const vec3f &accRef,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagneticAndGravityReferenceVectors(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		magRef,
  		accRef);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  FilterMeasurementsVarianceParametersRegister VnSensor::readFilterMeasurementsVarianceParameters()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	FilterMeasurementsVarianceParametersRegister reg;
  	response.parseFilterMeasurementsVarianceParameters(
  		&reg.angularWalkVariance,
  		&reg.angularRateVariance,
  		&reg.magneticVariance,
  		&reg.accelerationVariance);
  
  	return reg;
  }
  
  void VnSensor::writeFilterMeasurementsVarianceParameters(FilterMeasurementsVarianceParametersRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterMeasurementsVarianceParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.angularRateVariance, fields.magneticVariance, fields.accelerationVariance);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeFilterMeasurementsVarianceParameters(
  	const float &angularWalkVariance,
  	const vec3f &angularRateVariance,
  	const vec3f &magneticVariance,
  	const vec3f &accelerationVariance,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterMeasurementsVarianceParameters(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		angularWalkVariance,
  		angularRateVariance,
  		magneticVariance,
  		accelerationVariance);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  MagnetometerCompensationRegister VnSensor::readMagnetometerCompensation()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	MagnetometerCompensationRegister reg;
  	response.parseMagnetometerCompensation(
  		&reg.c,
  		&reg.b);
  
  	return reg;
  }
  
  void VnSensor::writeMagnetometerCompensation(MagnetometerCompensationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagnetometerCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeMagnetometerCompensation(
  	const mat3f &c,
  	const vec3f &b,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagnetometerCompensation(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		c,
  		b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  FilterActiveTuningParametersRegister VnSensor::readFilterActiveTuningParameters()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	FilterActiveTuningParametersRegister reg;
  	response.parseFilterActiveTuningParameters(
  		&reg.magneticDisturbanceGain,
  		&reg.accelerationDisturbanceGain,
  		&reg.magneticDisturbanceMemory,
  		&reg.accelerationDisturbanceMemory);
  
  	return reg;
  }
  
  void VnSensor::writeFilterActiveTuningParameters(FilterActiveTuningParametersRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterActiveTuningParameters(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magneticDisturbanceGain, fields.accelerationDisturbanceGain, fields.magneticDisturbanceMemory, fields.accelerationDisturbanceMemory);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeFilterActiveTuningParameters(
  	const float &magneticDisturbanceGain,
  	const float &accelerationDisturbanceGain,
  	const float &magneticDisturbanceMemory,
  	const float &accelerationDisturbanceMemory,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterActiveTuningParameters(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		magneticDisturbanceGain,
  		accelerationDisturbanceGain,
  		magneticDisturbanceMemory,
  		accelerationDisturbanceMemory);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  AccelerationCompensationRegister VnSensor::readAccelerationCompensation()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	AccelerationCompensationRegister reg;
  	response.parseAccelerationCompensation(
  		&reg.c,
  		&reg.b);
  
  	return reg;
  }
  
  void VnSensor::writeAccelerationCompensation(AccelerationCompensationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteAccelerationCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeAccelerationCompensation(
  	const mat3f &c,
  	const vec3f &b,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteAccelerationCompensation(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		c,
  		b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  mat3f VnSensor::readReferenceFrameRotation()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	mat3f reg;
  	response.parseReferenceFrameRotation(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeReferenceFrameRotation(const mat3f &c, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteReferenceFrameRotation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), c);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  YawPitchRollMagneticAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollMagneticAccelerationAndAngularRates()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadYawPitchRollMagneticAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	YawPitchRollMagneticAccelerationAndAngularRatesRegister reg;
  	response.parseYawPitchRollMagneticAccelerationAndAngularRates(
  		&reg.yawPitchRoll,
  		&reg.mag,
  		&reg.accel,
  		&reg.gyro);
  
  	return reg;
  }
  
  CommunicationProtocolControlRegister VnSensor::readCommunicationProtocolControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	CommunicationProtocolControlRegister reg;
  	uint8_t serialCount;
  	uint8_t serialStatus;
  	uint8_t spiCount;
  	uint8_t spiStatus;
  	uint8_t serialChecksum;
  	uint8_t spiChecksum;
  	uint8_t errorMode;
  	response.parseCommunicationProtocolControl(
  		&serialCount,
  		&serialStatus,
  		&spiCount,
  		&spiStatus,
  		&serialChecksum,
  		&spiChecksum,
  		&errorMode);
  
  	reg.serialCount = static_cast<CountMode>(serialCount);
  	reg.serialStatus = static_cast<StatusMode>(serialStatus);
  	reg.spiCount = static_cast<CountMode>(spiCount);
  	reg.spiStatus = static_cast<StatusMode>(spiStatus);
  	reg.serialChecksum = static_cast<ChecksumMode>(serialChecksum);
  	reg.spiChecksum = static_cast<ChecksumMode>(spiChecksum);
  	reg.errorMode = static_cast<ErrorMode>(errorMode);
  
  	return reg;
  }
  
  void VnSensor::writeCommunicationProtocolControl(CommunicationProtocolControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteCommunicationProtocolControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.serialCount, fields.serialStatus, fields.spiCount, fields.spiStatus, fields.serialChecksum, fields.spiChecksum, fields.errorMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeCommunicationProtocolControl(
  	CountMode serialCount,
  	StatusMode serialStatus,
  	CountMode spiCount,
  	StatusMode spiStatus,
  	ChecksumMode serialChecksum,
  	ChecksumMode spiChecksum,
  	ErrorMode errorMode,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteCommunicationProtocolControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		serialCount,
  		serialStatus,
  		spiCount,
  		spiStatus,
  		serialChecksum,
  		spiChecksum,
  		errorMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  SynchronizationControlRegister VnSensor::readSynchronizationControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	SynchronizationControlRegister reg;
  	uint8_t syncInMode;
  	uint8_t syncInEdge;
  	uint8_t syncOutMode;
  	uint8_t syncOutPolarity;
  	response.parseSynchronizationControl(
  		&syncInMode,
  		&syncInEdge,
  		&reg.syncInSkipFactor,
  		&syncOutMode,
  		&syncOutPolarity,
  		&reg.syncOutSkipFactor,
  		&reg.syncOutPulseWidth);
  
  	reg.syncInMode = static_cast<SyncInMode>(syncInMode);
  	reg.syncInEdge = static_cast<SyncInEdge>(syncInEdge);
  	reg.syncOutMode = static_cast<SyncOutMode>(syncOutMode);
  	reg.syncOutPolarity = static_cast<SyncOutPolarity>(syncOutPolarity);
  
  	return reg;
  }
  
  void VnSensor::writeSynchronizationControl(SynchronizationControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteSynchronizationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInMode, fields.syncInEdge, fields.syncInSkipFactor, fields.syncOutMode, fields.syncOutPolarity, fields.syncOutSkipFactor, fields.syncOutPulseWidth);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeSynchronizationControl(
  	SyncInMode syncInMode,
  	SyncInEdge syncInEdge,
  	const uint16_t &syncInSkipFactor,
  	SyncOutMode syncOutMode,
  	SyncOutPolarity syncOutPolarity,
  	const uint16_t &syncOutSkipFactor,
  	const uint32_t &syncOutPulseWidth,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteSynchronizationControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		syncInMode,
  		syncInEdge,
  		syncInSkipFactor,
  		syncOutMode,
  		syncOutPolarity,
  		syncOutSkipFactor,
  		syncOutPulseWidth);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  SynchronizationStatusRegister VnSensor::readSynchronizationStatus()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	SynchronizationStatusRegister reg;
  	response.parseSynchronizationStatus(
  		&reg.syncInCount,
  		&reg.syncInTime,
  		&reg.syncOutCount);
  
  	return reg;
  }
  
  void VnSensor::writeSynchronizationStatus(SynchronizationStatusRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteSynchronizationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.syncInCount, fields.syncInTime, fields.syncOutCount);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeSynchronizationStatus(
  	const uint32_t &syncInCount,
  	const uint32_t &syncInTime,
  	const uint32_t &syncOutCount,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteSynchronizationStatus(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		syncInCount,
  		syncInTime,
  		syncOutCount);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  FilterBasicControlRegister VnSensor::readFilterBasicControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	FilterBasicControlRegister reg;
  	uint8_t magMode;
  	uint8_t extMagMode;
  	uint8_t extAccMode;
  	uint8_t extGyroMode;
  	response.parseFilterBasicControl(
  		&magMode,
  		&extMagMode,
  		&extAccMode,
  		&extGyroMode,
  		&reg.gyroLimit);
  
  	reg.magMode = static_cast<MagneticMode>(magMode);
  	reg.extMagMode = static_cast<ExternalSensorMode>(extMagMode);
  	reg.extAccMode = static_cast<ExternalSensorMode>(extAccMode);
  	reg.extGyroMode = static_cast<ExternalSensorMode>(extGyroMode);
  
  	return reg;
  }
  
  void VnSensor::writeFilterBasicControl(FilterBasicControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magMode, fields.extMagMode, fields.extAccMode, fields.extGyroMode, fields.gyroLimit);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeFilterBasicControl(
  	MagneticMode magMode,
  	ExternalSensorMode extMagMode,
  	ExternalSensorMode extAccMode,
  	ExternalSensorMode extGyroMode,
  	const vec3f &gyroLimit,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterBasicControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		magMode,
  		extMagMode,
  		extAccMode,
  		extGyroMode,
  		gyroLimit);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  HeaveConfigurationRegister VnSensor::readHeaveConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	HeaveConfigurationRegister reg;
  	response.parseHeaveConfiguration(
  		&reg.initialWavePeriod,
  		&reg.initialWaveAmplitude,
  		&reg.maxWavePeriod,
  		&reg.minWaveAmplitude,
  		&reg.delayedHeaveCutoffFreq,
  		&reg.heaveCutoffFreq,
  		&reg.heaveRateCutoffFreq);
  
  	return reg;
  }
  
  void VnSensor::writeHeaveConfiguration(HeaveConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), 
  													fields.initialWavePeriod, 
  													fields.initialWaveAmplitude, 
  													fields.maxWavePeriod,
  													fields.minWaveAmplitude,
  													fields.delayedHeaveCutoffFreq,
  													fields.heaveCutoffFreq,
  													fields.heaveRateCutoffFreq);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeHeaveConfiguration(
  	const float &initialWavePeriod,
  	const float &initialWaveAmplitude,
  	const float &maxWavePeriod,
  	const float &minWaveAmplitude,
  	const float &delayedHeaveCutoffFreq,
  	const float &heaveCutoffFreq,
  	const float &heaveRateCutoffFreq,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteHeaveConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), 
  													initialWavePeriod, 
  													initialWaveAmplitude, 
  													maxWavePeriod,
  													minWaveAmplitude,
  													delayedHeaveCutoffFreq,
  													heaveCutoffFreq,
  													heaveRateCutoffFreq);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeBasicControlRegister VnSensor::readVpeBasicControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeBasicControlRegister reg;
  	uint8_t enable;
  	uint8_t headingMode;
  	uint8_t filteringMode;
  	uint8_t tuningMode;
  	response.parseVpeBasicControl(
  		&enable,
  		&headingMode,
  		&filteringMode,
  		&tuningMode);
  
  	reg.enable = static_cast<VpeEnable>(enable);
  	reg.headingMode = static_cast<HeadingMode>(headingMode);
  	reg.filteringMode = static_cast<VpeMode>(filteringMode);
  	reg.tuningMode = static_cast<VpeMode>(tuningMode);
  
  	return reg;
  }
  
  void VnSensor::writeVpeBasicControl(VpeBasicControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeBasicControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.enable, fields.headingMode, fields.filteringMode, fields.tuningMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeBasicControl(
  	VpeEnable enable,
  	HeadingMode headingMode,
  	VpeMode filteringMode,
  	VpeMode tuningMode,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeBasicControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		enable,
  		headingMode,
  		filteringMode,
  		tuningMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeMagnetometerBasicTuningRegister VnSensor::readVpeMagnetometerBasicTuning()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeMagnetometerBasicTuningRegister reg;
  	response.parseVpeMagnetometerBasicTuning(
  		&reg.baseTuning,
  		&reg.adaptiveTuning,
  		&reg.adaptiveFiltering);
  
  	return reg;
  }
  
  void VnSensor::writeVpeMagnetometerBasicTuning(VpeMagnetometerBasicTuningRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeMagnetometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeMagnetometerBasicTuning(
  	const vec3f &baseTuning,
  	const vec3f &adaptiveTuning,
  	const vec3f &adaptiveFiltering,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeMagnetometerBasicTuning(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		baseTuning,
  		adaptiveTuning,
  		adaptiveFiltering);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeMagnetometerAdvancedTuningRegister VnSensor::readVpeMagnetometerAdvancedTuning()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeMagnetometerAdvancedTuningRegister reg;
  	response.parseVpeMagnetometerAdvancedTuning(
  		&reg.minFiltering,
  		&reg.maxFiltering,
  		&reg.maxAdaptRate,
  		&reg.disturbanceWindow,
  		&reg.maxTuning);
  
  	return reg;
  }
  
  void VnSensor::writeVpeMagnetometerAdvancedTuning(VpeMagnetometerAdvancedTuningRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeMagnetometerAdvancedTuning(
  	const vec3f &minFiltering,
  	const vec3f &maxFiltering,
  	const float &maxAdaptRate,
  	const float &disturbanceWindow,
  	const float &maxTuning,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeMagnetometerAdvancedTuning(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		minFiltering,
  		maxFiltering,
  		maxAdaptRate,
  		disturbanceWindow,
  		maxTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeAccelerometerBasicTuningRegister VnSensor::readVpeAccelerometerBasicTuning()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeAccelerometerBasicTuningRegister reg;
  	response.parseVpeAccelerometerBasicTuning(
  		&reg.baseTuning,
  		&reg.adaptiveTuning,
  		&reg.adaptiveFiltering);
  
  	return reg;
  }
  
  void VnSensor::writeVpeAccelerometerBasicTuning(VpeAccelerometerBasicTuningRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeAccelerometerBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.baseTuning, fields.adaptiveTuning, fields.adaptiveFiltering);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeAccelerometerBasicTuning(
  	const vec3f &baseTuning,
  	const vec3f &adaptiveTuning,
  	const vec3f &adaptiveFiltering,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeAccelerometerBasicTuning(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		baseTuning,
  		adaptiveTuning,
  		adaptiveFiltering);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeAccelerometerAdvancedTuningRegister VnSensor::readVpeAccelerometerAdvancedTuning()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeAccelerometerAdvancedTuningRegister reg;
  	response.parseVpeAccelerometerAdvancedTuning(
  		&reg.minFiltering,
  		&reg.maxFiltering,
  		&reg.maxAdaptRate,
  		&reg.disturbanceWindow,
  		&reg.maxTuning);
  
  	return reg;
  }
  
  void VnSensor::writeVpeAccelerometerAdvancedTuning(VpeAccelerometerAdvancedTuningRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.minFiltering, fields.maxFiltering, fields.maxAdaptRate, fields.disturbanceWindow, fields.maxTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeAccelerometerAdvancedTuning(
  	const vec3f &minFiltering,
  	const vec3f &maxFiltering,
  	const float &maxAdaptRate,
  	const float &disturbanceWindow,
  	const float &maxTuning,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeAccelerometerAdvancedTuning(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		minFiltering,
  		maxFiltering,
  		maxAdaptRate,
  		disturbanceWindow,
  		maxTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VpeGyroBasicTuningRegister VnSensor::readVpeGyroBasicTuning()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVpeGyroBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VpeGyroBasicTuningRegister reg;
  	response.parseVpeGyroBasicTuning(
  		&reg.angularWalkVariance,
  		&reg.baseTuning,
  		&reg.adaptiveTuning);
  
  	return reg;
  }
  
  void VnSensor::writeVpeGyroBasicTuning(VpeGyroBasicTuningRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeGyroBasicTuning(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.angularWalkVariance, fields.baseTuning, fields.adaptiveTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVpeGyroBasicTuning(
  	const vec3f &angularWalkVariance,
  	const vec3f &baseTuning,
  	const vec3f &adaptiveTuning,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVpeGyroBasicTuning(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		angularWalkVariance,
  		baseTuning,
  		adaptiveTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  vec3f VnSensor::readFilterStartupGyroBias()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseFilterStartupGyroBias(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeFilterStartupGyroBias(const vec3f &bias, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteFilterStartupGyroBias(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), bias);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  MagnetometerCalibrationControlRegister VnSensor::readMagnetometerCalibrationControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	MagnetometerCalibrationControlRegister reg;
  	uint8_t hsiMode;
  	uint8_t hsiOutput;
  	response.parseMagnetometerCalibrationControl(
  		&hsiMode,
  		&hsiOutput,
  		&reg.convergeRate);
  
  	reg.hsiMode = static_cast<HsiMode>(hsiMode);
  	reg.hsiOutput = static_cast<HsiOutput>(hsiOutput);
  
  	return reg;
  }
  
  void VnSensor::writeMagnetometerCalibrationControl(MagnetometerCalibrationControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagnetometerCalibrationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.hsiMode, fields.hsiOutput, fields.convergeRate);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeMagnetometerCalibrationControl(
  	HsiMode hsiMode,
  	HsiOutput hsiOutput,
  	const uint8_t &convergeRate,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteMagnetometerCalibrationControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		hsiMode,
  		hsiOutput,
  		convergeRate);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  CalculatedMagnetometerCalibrationRegister VnSensor::readCalculatedMagnetometerCalibration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadCalculatedMagnetometerCalibration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	CalculatedMagnetometerCalibrationRegister reg;
  	response.parseCalculatedMagnetometerCalibration(
  		&reg.c,
  		&reg.b);
  
  	return reg;
  }
  
  float VnSensor::readIndoorHeadingModeControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	float reg;
  	response.parseIndoorHeadingModeControl(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeIndoorHeadingModeControl(const float &maxRateError, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteIndoorHeadingModeControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), maxRateError);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  vec3f VnSensor::readVelocityCompensationMeasurement()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseVelocityCompensationMeasurement(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeVelocityCompensationMeasurement(const vec3f &velocity, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVelocityCompensationMeasurement(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), velocity);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VelocityCompensationControlRegister VnSensor::readVelocityCompensationControl()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VelocityCompensationControlRegister reg;
  	uint8_t mode;
  	response.parseVelocityCompensationControl(
  		&mode,
  		&reg.velocityTuning,
  		&reg.rateTuning);
  
  	reg.mode = static_cast<VelocityCompensationMode>(mode);
  
  	return reg;
  }
  
  void VnSensor::writeVelocityCompensationControl(VelocityCompensationControlRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVelocityCompensationControl(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.velocityTuning, fields.rateTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeVelocityCompensationControl(
  	VelocityCompensationMode mode,
  	const float &velocityTuning,
  	const float &rateTuning,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteVelocityCompensationControl(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		mode,
  		velocityTuning,
  		rateTuning);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  VelocityCompensationStatusRegister VnSensor::readVelocityCompensationStatus()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadVelocityCompensationStatus(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	VelocityCompensationStatusRegister reg;
  	response.parseVelocityCompensationStatus(
  		&reg.x,
  		&reg.xDot,
  		&reg.accelOffset,
  		&reg.omega);
  
  	return reg;
  }
  
  ImuMeasurementsRegister VnSensor::readImuMeasurements()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadImuMeasurements(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	ImuMeasurementsRegister reg;
  	response.parseImuMeasurements(
  		&reg.mag,
  		&reg.accel,
  		&reg.gyro,
  		&reg.temp,
  		&reg.pressure);
  
  	return reg;
  }
  
  GpsConfigurationRegister VnSensor::readGpsConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GpsConfigurationRegister reg;
  	uint8_t mode;
  	uint8_t ppsSource;
  	uint8_t rate;
  	uint8_t antPow;
  	response.parseGpsConfiguration(
  		&mode,
  		&ppsSource,
  		&rate,
  		&antPow);
  
  	reg.mode = static_cast<GpsMode>(mode);
  	reg.ppsSource = static_cast<PpsSource>(ppsSource);
  	reg.rate = static_cast<GpsRate>(rate);
  	reg.antPow = static_cast<AntPower>(antPow);
  
  	return reg;
  }
  
  void VnSensor::writeGpsConfiguration(GpsConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.mode, fields.ppsSource, fields.rate, fields.antPow);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeGpsConfiguration(
  	GpsMode mode,
  	PpsSource ppsSource,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		mode,
  		ppsSource);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeGpsConfiguration(
  	GpsMode mode,
  	PpsSource ppsSource,
  	GpsRate rate,
  	AntPower antPow,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		mode,
  		ppsSource,
  		rate,
  		antPow);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  vec3f VnSensor::readGpsAntennaOffset()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	vec3f reg;
  	response.parseGpsAntennaOffset(&reg);
  
  	return reg;
  }
  
  void VnSensor::writeGpsAntennaOffset(const vec3f &position, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsAntennaOffset(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), position);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  GpsSolutionLlaRegister VnSensor::readGpsSolutionLla()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GpsSolutionLlaRegister reg;
  	uint8_t gpsFix;
  	response.parseGpsSolutionLla(
  		&reg.time,
  		&reg.week,
  		&gpsFix,
  		&reg.numSats,
  		&reg.lla,
  		&reg.nedVel,
  		&reg.nedAcc,
  		&reg.speedAcc,
  		&reg.timeAcc);
  
  	reg.gpsFix = static_cast<GpsFix>(gpsFix);
  
  	return reg;
  }
  
  GpsSolutionEcefRegister VnSensor::readGpsSolutionEcef()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GpsSolutionEcefRegister reg;
  	uint8_t gpsFix;
  	response.parseGpsSolutionEcef(
  		&reg.tow,
  		&reg.week,
  		&gpsFix,
  		&reg.numSats,
  		&reg.position,
  		&reg.velocity,
  		&reg.posAcc,
  		&reg.speedAcc,
  		&reg.timeAcc);
  
  	reg.gpsFix = static_cast<GpsFix>(gpsFix);
  
  	return reg;
  }
  
  InsSolutionLlaRegister VnSensor::readInsSolutionLla()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsSolutionLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsSolutionLlaRegister reg;
  	response.parseInsSolutionLla(
  		&reg.time,
  		&reg.week,
  		&reg.status,
  		&reg.yawPitchRoll,
  		&reg.position,
  		&reg.nedVel,
  		&reg.attUncertainty,
  		&reg.posUncertainty,
  		&reg.velUncertainty);
  
  	return reg;
  }
  
  InsSolutionEcefRegister VnSensor::readInsSolutionEcef()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsSolutionEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsSolutionEcefRegister reg;
  	response.parseInsSolutionEcef(
  		&reg.time,
  		&reg.week,
  		&reg.status,
  		&reg.yawPitchRoll,
  		&reg.position,
  		&reg.velocity,
  		&reg.attUncertainty,
  		&reg.posUncertainty,
  		&reg.velUncertainty);
  
  	return reg;
  }
  
  InsAdvancedConfigurationRegister VnSensor::readInsAdvancedConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsAdvancedConfigurationRegister reg;
  	uint8_t useMag;
  	uint8_t usePres;
  	uint8_t posAtt;
  	uint8_t velAtt;
  	uint8_t velBias;
  	uint8_t useFoam;
  	response.parseInsAdvancedConfiguration(
  		&useMag,
  		&usePres,
  		&posAtt,
  		&velAtt,
  		&velBias,
  		&useFoam,
  		&reg.gpsCovType,
  		&reg.velCount,
  		&reg.velInit,
  		&reg.moveOrigin,
  		&reg.gpsTimeout,
  		&reg.deltaLimitPos,
  		&reg.deltaLimitVel,
  		&reg.minPosUncertainty,
  		&reg.minVelUncertainty);
  
  	reg.useMag = useMag != 0;
  	reg.usePres = usePres != 0;
  	reg.posAtt = posAtt != 0;
  	reg.velAtt = velAtt != 0;
  	reg.velBias = velBias != 0;
  	reg.useFoam = static_cast<FoamInit>(useFoam);
  
  	return reg;
  }
  
  void VnSensor::writeInsAdvancedConfiguration(InsAdvancedConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsAdvancedConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMag, fields.usePres, fields.posAtt, fields.velAtt, fields.velBias, fields.useFoam, fields.gpsCovType, fields.velCount, fields.velInit, fields.moveOrigin, fields.gpsTimeout, fields.deltaLimitPos, fields.deltaLimitVel, fields.minPosUncertainty, fields.minVelUncertainty);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeInsAdvancedConfiguration(
  	const uint8_t &useMag,
  	const uint8_t &usePres,
  	const uint8_t &posAtt,
  	const uint8_t &velAtt,
  	const uint8_t &velBias,
  	FoamInit useFoam,
  	const uint8_t &gpsCovType,
  	const uint8_t &velCount,
  	const float &velInit,
  	const float &moveOrigin,
  	const float &gpsTimeout,
  	const float &deltaLimitPos,
  	const float &deltaLimitVel,
  	const float &minPosUncertainty,
  	const float &minVelUncertainty,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteInsAdvancedConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		useMag,
  		usePres,
  		posAtt,
  		velAtt,
  		velBias,
  		useFoam,
  		gpsCovType,
  		velCount,
  		velInit,
  		moveOrigin,
  		gpsTimeout,
  		deltaLimitPos,
  		deltaLimitVel,
  		minPosUncertainty,
  		minVelUncertainty);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  InsStateLlaRegister VnSensor::readInsStateLla()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsStateLla(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsStateLlaRegister reg;
  	response.parseInsStateLla(
  		&reg.yawPitchRoll,
  		&reg.position,
  		&reg.velocity,
  		&reg.accel,
  		&reg.angularRate);
  
  	return reg;
  }
  
  InsStateEcefRegister VnSensor::readInsStateEcef()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadInsStateEcef(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	InsStateEcefRegister reg;
  	response.parseInsStateEcef(
  		&reg.yawPitchRoll,
  		&reg.position,
  		&reg.velocity,
  		&reg.accel,
  		&reg.angularRate);
  
  	return reg;
  }
  
  StartupFilterBiasEstimateRegister VnSensor::readStartupFilterBiasEstimate()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	StartupFilterBiasEstimateRegister reg;
  	response.parseStartupFilterBiasEstimate(
  		&reg.gyroBias,
  		&reg.accelBias,
  		&reg.pressureBias);
  
  	return reg;
  }
  
  void VnSensor::writeStartupFilterBiasEstimate(StartupFilterBiasEstimateRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteStartupFilterBiasEstimate(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.gyroBias, fields.accelBias, fields.pressureBias);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeStartupFilterBiasEstimate(
  	const vec3f &gyroBias,
  	const vec3f &accelBias,
  	const float &pressureBias,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteStartupFilterBiasEstimate(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		gyroBias,
  		accelBias,
  		pressureBias);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  DeltaThetaAndDeltaVelocityRegister VnSensor::readDeltaThetaAndDeltaVelocity()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadDeltaThetaAndDeltaVelocity(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	DeltaThetaAndDeltaVelocityRegister reg;
  	response.parseDeltaThetaAndDeltaVelocity(
  		&reg.deltaTime,
  		&reg.deltaTheta,
  		&reg.deltaVelocity);
  
  	return reg;
  }
  
  DeltaThetaAndDeltaVelocityConfigurationRegister VnSensor::readDeltaThetaAndDeltaVelocityConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	DeltaThetaAndDeltaVelocityConfigurationRegister reg;
  	uint8_t integrationFrame;
  	uint8_t gyroCompensation;
  	uint8_t accelCompensation;
  	uint8_t earthRateCorrection;
  	response.parseDeltaThetaAndDeltaVelocityConfiguration(
  		&integrationFrame,
  		&gyroCompensation,
  		&accelCompensation,
  		&earthRateCorrection);
  
  	reg.integrationFrame = static_cast<IntegrationFrame>(integrationFrame);
  	reg.gyroCompensation = static_cast<CompensationMode>(gyroCompensation);
  	reg.accelCompensation = static_cast<AccCompensationMode>(accelCompensation);
  	reg.earthRateCorrection = static_cast<EarthRateCorrection>(earthRateCorrection);
  
  	return reg;
  }
  
  void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration(DeltaThetaAndDeltaVelocityConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.integrationFrame, fields.gyroCompensation, fields.accelCompensation, fields.earthRateCorrection);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration(
  	IntegrationFrame integrationFrame,
  	CompensationMode gyroCompensation,
  	AccCompensationMode accelCompensation,
  	bool waitForReply)
  {
  	writeDeltaThetaAndDeltaVelocityConfiguration(
  		integrationFrame,
  		gyroCompensation,
  		accelCompensation,
  		protocol::uart::EARTHRATECORR_NONE,
  		waitForReply);
  }
  
  void VnSensor::writeDeltaThetaAndDeltaVelocityConfiguration(
  	IntegrationFrame integrationFrame,
  	CompensationMode gyroCompensation,
  	AccCompensationMode accelCompensation,
  	EarthRateCorrection earthRateCorrection,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteDeltaThetaAndDeltaVelocityConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		integrationFrame,
  		gyroCompensation,
  		accelCompensation,
  		earthRateCorrection);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  ReferenceVectorConfigurationRegister VnSensor::readReferenceVectorConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	ReferenceVectorConfigurationRegister reg;
  	uint8_t useMagModel;
  	uint8_t useGravityModel;
  	response.parseReferenceVectorConfiguration(
  		&useMagModel,
  		&useGravityModel,
  		&reg.recalcThreshold,
  		&reg.year,
  		&reg.position);
  
  	reg.useMagModel = useMagModel != 0;
  	reg.useGravityModel = useGravityModel != 0;
  
  	return reg;
  }
  
  void VnSensor::writeReferenceVectorConfiguration(ReferenceVectorConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteReferenceVectorConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.useMagModel, fields.useGravityModel, fields.recalcThreshold, fields.year, fields.position);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeReferenceVectorConfiguration(
  	const uint8_t &useMagModel,
  	const uint8_t &useGravityModel,
  	const uint32_t &recalcThreshold,
  	const float &year,
  	const vec3d &position,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteReferenceVectorConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		useMagModel,
  		useGravityModel,
  		recalcThreshold,
  		year,
  		position);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  GyroCompensationRegister VnSensor::readGyroCompensation()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GyroCompensationRegister reg;
  	response.parseGyroCompensation(
  		&reg.c,
  		&reg.b);
  
  	return reg;
  }
  
  void VnSensor::writeGyroCompensation(GyroCompensationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGyroCompensation(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.c, fields.b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeGyroCompensation(
  	const mat3f &c,
  	const vec3f &b,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGyroCompensation(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		c,
  		b);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  ImuFilteringConfigurationRegister VnSensor::readImuFilteringConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	ImuFilteringConfigurationRegister reg;
  	uint8_t magFilterMode;
  	uint8_t accelFilterMode;
  	uint8_t gyroFilterMode;
  	uint8_t tempFilterMode;
  	uint8_t presFilterMode;
  	response.parseImuFilteringConfiguration(
  		&reg.magWindowSize,
  		&reg.accelWindowSize,
  		&reg.gyroWindowSize,
  		&reg.tempWindowSize,
  		&reg.presWindowSize,
  		&magFilterMode,
  		&accelFilterMode,
  		&gyroFilterMode,
  		&tempFilterMode,
  		&presFilterMode);
  
  	reg.magFilterMode = static_cast<FilterMode>(magFilterMode);
  	reg.accelFilterMode = static_cast<FilterMode>(accelFilterMode);
  	reg.gyroFilterMode = static_cast<FilterMode>(gyroFilterMode);
  	reg.tempFilterMode = static_cast<FilterMode>(tempFilterMode);
  	reg.presFilterMode = static_cast<FilterMode>(presFilterMode);
  
  	return reg;
  }
  
  void VnSensor::writeImuFilteringConfiguration(ImuFilteringConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteImuFilteringConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.magWindowSize, fields.accelWindowSize, fields.gyroWindowSize, fields.tempWindowSize, fields.presWindowSize, fields.magFilterMode, fields.accelFilterMode, fields.gyroFilterMode, fields.tempFilterMode, fields.presFilterMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeImuFilteringConfiguration(
  	const uint16_t &magWindowSize,
  	const uint16_t &accelWindowSize,
  	const uint16_t &gyroWindowSize,
  	const uint16_t &tempWindowSize,
  	const uint16_t &presWindowSize,
  	FilterMode magFilterMode,
  	FilterMode accelFilterMode,
  	FilterMode gyroFilterMode,
  	FilterMode tempFilterMode,
  	FilterMode presFilterMode,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteImuFilteringConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		magWindowSize,
  		accelWindowSize,
  		gyroWindowSize,
  		tempWindowSize,
  		presWindowSize,
  		magFilterMode,
  		accelFilterMode,
  		gyroFilterMode,
  		tempFilterMode,
  		presFilterMode);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  GpsCompassBaselineRegister VnSensor::readGpsCompassBaseline()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GpsCompassBaselineRegister reg;
  	response.parseGpsCompassBaseline(
  		&reg.position,
  		&reg.uncertainty);
  
  	return reg;
  }
  
  void VnSensor::writeGpsCompassBaseline(GpsCompassBaselineRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsCompassBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.position, fields.uncertainty);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeGpsCompassBaseline(
  	const vec3f &position,
  	const vec3f &uncertainty,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteGpsCompassBaseline(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		position,
  		uncertainty);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  GpsCompassEstimatedBaselineRegister VnSensor::readGpsCompassEstimatedBaseline()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadGpsCompassEstimatedBaseline(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	GpsCompassEstimatedBaselineRegister reg;
  	uint8_t estBaselineUsed;
  	response.parseGpsCompassEstimatedBaseline(
  		&estBaselineUsed,
  		&reg.numMeas,
  		&reg.position,
  		&reg.uncertainty);
  
  	reg.estBaselineUsed = estBaselineUsed != 0;
  
  	return reg;
  }
  
  ImuRateConfigurationRegister VnSensor::readImuRateConfiguration()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	ImuRateConfigurationRegister reg;
  	response.parseImuRateConfiguration(
  		&reg.imuRate,
  		&reg.navDivisor,
  		&reg.filterTargetRate,
  		&reg.filterMinRate);
  
  	return reg;
  }
  
  void VnSensor::writeImuRateConfiguration(ImuRateConfigurationRegister &fields, bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteImuRateConfiguration(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend), fields.imuRate, fields.navDivisor, fields.filterTargetRate, fields.filterMinRate);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  void VnSensor::writeImuRateConfiguration(
  	const uint16_t &imuRate,
  	const uint16_t &navDivisor,
  	const float &filterTargetRate,
  	const float &filterMinRate,
  	bool waitForReply)
  {
  	char toSend[256];
  
  	size_t length = Packet::genWriteImuRateConfiguration(
  		_pi->_sendErrorDetectionMode,
  		toSend,
  		sizeof(toSend),
  		imuRate,
  		navDivisor,
  		filterTargetRate,
  		filterMinRate);
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response);
  }
  
  YawPitchRollTrueBodyAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueBodyAccelerationAndAngularRates()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadYawPitchRollTrueBodyAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	YawPitchRollTrueBodyAccelerationAndAngularRatesRegister reg;
  	response.parseYawPitchRollTrueBodyAccelerationAndAngularRates(
  		&reg.yawPitchRoll,
  		&reg.bodyAccel,
  		&reg.gyro);
  
  	return reg;
  }
  
  YawPitchRollTrueInertialAccelerationAndAngularRatesRegister VnSensor::readYawPitchRollTrueInertialAccelerationAndAngularRates()
  {
  	char toSend[17];
  
  	size_t length = Packet::genReadYawPitchRollTrueInertialAccelerationAndAngularRates(_pi->_sendErrorDetectionMode, toSend, sizeof(toSend));
  
  	Packet response;
  	_pi->transactionNoFinalize(toSend, length, true, &response);
  
  	YawPitchRollTrueInertialAccelerationAndAngularRatesRegister reg;
  	response.parseYawPitchRollTrueInertialAccelerationAndAngularRates(
  		&reg.yawPitchRoll,
  		&reg.inertialAccel,
  		&reg.gyro);
  
  	return reg;
  }
  
  void VnSensor::switchProcessors(VnProcessorType processor, std::string model, std::string firmware)
  {
  	std::string switchCmd = "";
  
  	if (model.find("VN-300") != std::string::npos)
  	{
  		switch (processor)
  		{
  		case VNPROCESSOR_GPS:
  			if ((model.find("SMD") != std::string::npos) || (model.find("DEV") != std::string::npos))
  			{
  				switchCmd = "$VNSPS,1,1,115200";
  			}
  			else if (model.find("CR") != std::string::npos)
  			{
  				switchCmd = "$VNDBS,3,1";
  			}
  			break;
  		}
  	} 
  	else if ((model.find("VN-110") != std::string::npos) || (model.find("VN-210") != std::string::npos) || (model.find("VN-310") != std::string::npos))
  	{
  		switch (processor)
  		{
  		case VNPROCESSOR_NAV:
  			switchCmd = "$VNSBL,0";
  			break;
  		case VNPROCESSOR_GPS:
  			if ((model.find("VN-310") != std::string::npos) || (model.find("VN-210E") != std::string::npos))
  			{
  				switchCmd = "$VNSBL,1";
  			}
  			break;
  		case VNPROCESSOR_IMU:
  			switchCmd = "$VNSBL,2";
  			break;
  		}
  	}
  
  	if (switchCmd.empty())
  	{
  		throw invalid_operation();
  	}
  
  	// Send switch command
  	char toSend[256];
  #if VN_HAVE_SECURE_CRT
  	strcpy_s(toSend, 256, switchCmd.c_str());
  #else
  	strcpy(toSend, switchCmd.c_str());
  #endif
  
  	size_t length = Packet::finalizeCommand(ERRORDETECTIONMODE_CRC, toSend, switchCmd.length());
  	Packet response;
  
  	_pi->transactionNoFinalize(toSend, length, true, &response, 6000, 6000);
  
  	// Wait 2 seconds to give the processor time to switch
  	Thread::sleepSec(2);
  }
  
  void VnSensor::firmwareUpdate(int baudRate, std::string fileName)
  {
  	std::string record = "";
  	
  	// Open firmware update file
  	openFirmwareUpdateFile(fileName);
  	
  	// Enter bootloader mode
  	firmwareUpdateMode(true);
  
  	// Give the sensor time to reboot into bootloader mode
  	Thread::sleepSec(1);
  
  	// Configure baudrate for firmware update
  	uint32_t navBaudRate = baudrate();
  	setFirmwareUpdateBaudRate(baudRate);
  
  	// Calibrate the bootloader by letting it calculate the current baudrate.
  	std::string bootloaderVersion = calibrateBootloader();
  	//cout << bootloaderVersion << endl;
  	
  	// Send each record from the firmware update file one at a time
  	do
  	{
  		record = getNextFirwareUpdateRecord();
  		if (!record.empty())
  		{
  			record.erase(0, 1); // Remove the ':' from the beginning of the line.
  			writeFirmwareUpdateRecord(record, true);
  		}
  	} while (!record.empty());
  	
  	// Close firmware update file
  	closeFirmwareUpdateFile();
  	
  	// Reset baudrate
  	setFirmwareUpdateBaudRate(navBaudRate);
  	
  	// Exit bootloader mode. Just sleep for 10 seconds
  	Thread::sleepSec(10);
  
  	// Do a reset
  	reset(true);
  
  	// Give the sensor time to recover after the reset
  	Thread::sleepSec(2);
  }
  
  void VnSensor::setFirmwareUpdateBaudRate(int baudRate)
  {
  	_pi->pSerialPort->changeBaudrate(baudRate);
  }
  
  void VnSensor::writeFirmwareUpdateRecord(const string& record, bool waitForReply)
  {
  	char toSend[MAXFIRMWAREUPDATERECORDSIZE +12];
  
  	size_t length = Packet::genWriteFirmwareUpdateRecord(ERRORDETECTIONMODE_CRC, toSend, sizeof(toSend), record.c_str());
  
  	Packet response;
  RESEND:
  	_pi->_filteringBootloaderResponses = true; /*Enable Bootloader Filtering just before sending the bootloader command */
  	_pi->transactionNoFinalize(toSend, length, waitForReply, &response, 6000, 6000);
  
  	BootloaderError responseCode = (BootloaderError)std::stoi(response.datastr().substr(7,2));
  
  
  	if (responseCode == BLE_COMM_ERROR)
  	{
  		// This is the only case where we will try again
  		goto RESEND;
  	}
  
  	if (responseCode != BLE_NONE)
  	{
  		// Abort
  		cout << vn::protocol::uart::str(responseCode) << endl;
  		throw unknown_error();
  	}
  }
  
  
  std::string VnSensor::calibrateBootloader()
  {
  	char baudratetest[] = "                  "; /* Only need 8, but doubling it just to be safe */
  	char bootloaderVersion[64];
  	memset(bootloaderVersion, 0, 64);
  	uint16_t responseTimeoutMs = 500;
  	uint16_t retransmitDelayMs = 500;
  
  	Packet response;
  	_pi->_filteringBootloaderResponses = true; /*Enable Bootloader Filtering just before sending the bootloader command */
  	_pi->transactionNoFinalize(baudratetest, sizeof(baudratetest)-2, true, &response, 7000, 7000);
  
  	std::string version = response.datastr();
  	return version;
  }
  
  std::string VnSensor::getNextFirwareUpdateRecord()
  {
  	string record = "";
  
  	if (firmwareUpdateFile != NULL)
  	{
  		char buffer[MAXFIRMWAREUPDATERECORDSIZE];
  #if VN_HAVE_SECURE_CRT
  		int result = fscanf_s(firmwareUpdateFile, "%s\n", buffer, MAXFIRMWAREUPDATERECORDSIZE);
  #else
  		int result = fscanf(firmwareUpdateFile, "%s\n", buffer);
  #endif
  
  		if (result != EOF)
  		{
  			record = buffer;
  		}
  	}
  
  	return record;
  }
  
  void VnSensor::openFirmwareUpdateFile(std::string filename)
  {
  #if VN_HAVE_SECURE_CRT
  	errno_t error = fopen_s(&firmwareUpdateFile, filename.c_str(), "r");
  #else
  	firmwareUpdateFile = fopen(filename.c_str(), "r");
  #endif
  
  	if (firmwareUpdateFile == NULL) {
  		throw unknown_error();
  	}
  }
  
  void VnSensor::closeFirmwareUpdateFile()
  {
  	if (firmwareUpdateFile != NULL)
  	{
  		fclose(firmwareUpdateFile);
  		firmwareUpdateFile = NULL;
  	}
  }
  
  
  #if PYTHON && !PL156_ORIGINAL && !PL156_FIX_ATTEMPT_1
  
  void VnSensor::stopRequest()
  {
  	_pi->port->stopThread();
  }
  
  bool VnSensor::threadStopped()
  {
  	return _pi->port->threadStopped();
  }
  
  void VnSensor::unregisterListners()
  {
  	_pi->port->unregisterDataReceivedHandler();
  }
  
  void VnSensor::shutdownRequest()
  {
  	if (_pi->DidWeOpenSimplePort)
  	{
  		_pi->port->close();
  	}
  
  	_pi->DidWeOpenSimplePort = false;
  
  	if (_pi->SimplePortIsOurs)
  	{
  		delete _pi->port;
  
  		_pi->port = NULL;
  	}
  
  }
  
  void VnSensor::goRequest()
  {
  	_pi->port->resumeThread();
  }
  
  #endif
  
  }
  }
  