This example illustrates basic connectivity and interaction with a VectorNav sensor using the low-level UART protocol code.
To compile and run for an environment not listed here, you will need to add all of the *.cpp
files in the directory <root>/cpp/src
along with the file located at <root>/c/examples/uart_protocol/main.c
to your project for compilation. You will also need to add <root>/cpp/include
to your include directories.
#include <iostream>
#include <string.h>
#include "vn/packet.h"
#include "vn/sensors.h"
using namespace std;
void validPacketFoundHandler(
void* userData,
Packet &packet,
size_t runningIndexOfPacketStart);
void UserUart_initialize();
void UserUart_mockReceivedData(char* buffer, size_t bufferSize);
void UserUart_mockReceivedData(string data);
bool UserUart_checkForReceivedData(char* buffer, size_t bufferSize, size_t* numOfBytesReceived);
void UserUart_sendData(char *data, size_t size);
bool gIsCheckingForModelNumberResponse = false;
bool gIsCheckingForAsyncOutputFreqResponse = false;
bool gIsCheckingForVpeBasicControlResponse = false;
uint8_t gEnable, gHeadingMode, gFilteringMode, gTuningMode;
int main(int argc, char *argv[])
{
char buffer[256];
size_t numOfBytes;
uint8_t mockBinaryAsyncData[] = { 0xFA, 0x01, 0x09, 0x00, 0x70, 0x05, 0x00, 0x03, 0x0A, 0x00, 0x00, 0x00, 0x48, 0x0E, 0x2C, 0x42, 0x08, 0x4C, 0x37, 0xC1, 0x10, 0x38, 0x8B, 0xC2, 0xD4, 0xCB };
UserUart_initialize();
UserUart_mockReceivedData("$VNYMR,+100.949,-007.330,+000.715,-00.0049,-00.2449,+00.5397,-01.258,-00.100,-09.701,-00.000018,+00.001122,-00.000551*69\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
numOfBytes = Packet::genReadModelNumber(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer));
gIsCheckingForModelNumberResponse = true;
UserUart_sendData(buffer, numOfBytes);
UserUart_mockReceivedData("$VNRRG,01,VN-200T-CR*31\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
gIsCheckingForModelNumberResponse = false;
numOfBytes = Packet::genWriteAsyncDataOutputFrequency(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer), 2);
gIsCheckingForAsyncOutputFreqResponse = true;
UserUart_sendData(buffer, numOfBytes);
UserUart_mockReceivedData("$VNWRG,07,2*6F\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
gIsCheckingForAsyncOutputFreqResponse = false;
cout << "Reading current values of the VPE Basic Control register." << endl;
numOfBytes = Packet::genReadVpeBasicControl(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer));
gIsCheckingForVpeBasicControlResponse = true;
UserUart_sendData(buffer, numOfBytes);
UserUart_mockReceivedData("$VNRRG,35,1,1,1,1*75\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
gIsCheckingForVpeBasicControlResponse = false;
cout << "Writing new values to the VPE Basic Control register." << endl;
numOfBytes = Packet::genWriteVpeBasicControl(
ERRORDETECTIONMODE_CHECKSUM,
buffer,
sizeof(buffer),
gEnable,
0,
gFilteringMode,
gTuningMode);
gIsCheckingForVpeBasicControlResponse = true;
UserUart_sendData(buffer, numOfBytes);
UserUart_mockReceivedData("$VNWRG,35,1,0,1,1*71\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
gIsCheckingForVpeBasicControlResponse = false;
numOfBytes = Packet::genWriteBinaryOutput1(
ERRORDETECTIONMODE_CHECKSUM,
buffer,
sizeof(buffer),
ASYNCMODE_PORT1,
200,
COMMONGROUP_TIMESTARTUP | COMMONGROUP_YAWPITCHROLL,
TIMEGROUP_NONE,
IMUGROUP_NONE,
GPSGROUP_NONE,
ATTITUDEGROUP_NONE,
INSGROUP_NONE);
UserUart_sendData(buffer, numOfBytes);
UserUart_mockReceivedData(reinterpret_cast<char*>(mockBinaryAsyncData), sizeof(mockBinaryAsyncData));
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
UserUart_mockReceivedData("$VNERR,12*72\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
return 0;
}
void validPacketFoundHandler(
void* userData,
Packet &packet,
size_t runningIndexOfPacketStart)
{
if (packet.
type() == Packet::TYPE_ASCII)
{
{
if (asyncType == VNYMR)
{
vec3f ypr, mag, accel, angularRate;
packet.
parseVNYMR(&ypr, &mag, &accel, &angularRate);
cout << "[Found VNYMR Packet]" << endl;
cout << " YawPitchRoll: " << ypr << endl;
cout << " Magnetic: " << mag << endl;
cout << " Acceleration: " << accel << endl;
cout << " Angular Rate: " << angularRate << endl;
}
}
{
if (gIsCheckingForModelNumberResponse)
{
char modelNumber[100];
cout << "Model Number: " << modelNumber << endl;
}
else if (gIsCheckingForAsyncOutputFreqResponse)
{
uint32_t asyncOutputFreq;
cout << "Asynchronous Output Frequency: " << asyncOutputFreq << " Hz" << endl;
}
else if (gIsCheckingForVpeBasicControlResponse)
{
cout << "[VPE Basic Control]" << endl;
cout << " Enable: " << (gEnable ? "true" : "false") << endl;
cout << " Heading Mode: " << static_cast<vn::protocol::uart::HeadingMode>(gHeadingMode) << endl;
cout << " Filtering Mode: " << static_cast<vn::protocol::uart::FilterMode>(gFilteringMode) << endl;
cout << " Tuning Mode: " << static_cast<vn::protocol::uart::FilterMode>(gTuningMode) << endl;
}
}
{
cout <<
"Sensor Error: " << packet.
parseError() << endl;
}
}
else if (packet.
type() == Packet::TYPE_BINARY)
{
uint64_t timeStartup;
COMMONGROUP_TIMESTARTUP | COMMONGROUP_YAWPITCHROLL,
TIMEGROUP_NONE,
IMUGROUP_NONE,
GPSGROUP_NONE,
ATTITUDEGROUP_NONE,
INSGROUP_NONE))
{
return;
}
cout << "[Binary Packet Received]" << endl;
cout << " TimeStartup: " << timeStartup << endl;
cout << " Yaw Pitch Roll: " << ypr << endl;
}
}
char* mockUartReceivedDataBuffer[256];
size_t mockUartReceivedDataSize;
void UserUart_initialize()
{
mockUartReceivedDataSize = 0;
}
void UserUart_mockReceivedData(char* buffer, size_t bufferSize)
{
memcpy(mockUartReceivedDataBuffer, buffer, bufferSize);
mockUartReceivedDataSize = bufferSize;
}
void UserUart_mockReceivedData(string data)
{
memcpy(mockUartReceivedDataBuffer, data.c_str(), data.length());
mockUartReceivedDataSize = data.length();
}
bool UserUart_checkForReceivedData(char* buffer, size_t bufferSize, size_t* numOfBytesReceived)
{
if (mockUartReceivedDataSize == 0)
return false;
memcpy(buffer, mockUartReceivedDataBuffer, mockUartReceivedDataSize);
*numOfBytesReceived = mockUartReceivedDataSize;
mockUartReceivedDataSize = 0;
return true;
}
void UserUart_sendData(char *data, size_t size)
{
}