FaSTTUBe/INS-VN-200
54
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial

Browse Source
This commit is contained in:
r.koeppe
2024-05-14 02:14:13 +02:00
parent 0052d3984b
commit 2d22ccd2d6
1423 changed files with 354055 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

381
vnproglib/cpp/examples/uart_protocol/main.cpp Normal file
View File

@ -0,0 +1,381 @@
#include <iostream>
#include <string.h>
// Include this header file to get access to VectorNav UART protocol methods.
#include "vn/packet.h"
#include "vn/sensors.h"
using namespace std;
using namespace vn::math;
using namespace vn::protocol::uart;
// Method declarations for future use.
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 };
// This example provides an overview of the UART protocol functionality
// of the VectorNav C++ Library.
// Using the UART Protocol allows communicating with a VectorNav sensor
// over a UART interface using both ASCII and binary protocols. Usage of
// just the UART Protocol features without the VnSensor object requires you
// to do all of the grunt work of initializing and managing the UART port
// for your development environment. Once this is setup, you will
// initialize the UART protocol and then simply pass arrays of data between
// your UART code and the VectorNav C++ Library's protocol code. To keep
// this example generic, we will mock the necessary UART initialization and
// management functions that would need to be replaced by code specific for
// your environment to tie into a real VectorNav sensor. For now we just
// use some fake data to illustrate the process.
// The Protocol structure encapsulates the data used for buffering and
// handling incoming data. It is not associated with sending commands to
// the sensor. This will be illustrated further in the example.
PacketFinder pf;
// Register our callback method for when the PacketFinder finds an ASCII
// asynchronous packet.
pf.registerPossiblePacketFoundHandler(NULL, vn::protocol::uart::PacketFinder::ValidPacketFoundHandler(validPacketFoundHandler));
// Initialize the UART port (this is mimicked in this example).
UserUart_initialize();
// With our PacketFinder and mock UART initialized, we will fake an
// asynchronous message output by a VectorNav sensor and received by our
// mock UART port.
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");
// Normally you will be continually checking for new UART data and then
// passing any received data to the PacketFinder to build, parse and
// verify data packets. Since this is just for demonstration purposes, we
// just poll the mock UART for its current data. In a real world
// environment where you are servicing a real UART port, you would have
// code similar to the code lines below.
//
// char buffer[256];
// while (1)
// {
// size_t numOfBytes;
// if (UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes))
// VnUartPacketFinder_processReceivedData(buffer, numOfBytes);
// }
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
// Now when we pass the data to the PacketFinder, our function
// validPacketFoundHandler will be called since we passed in a complete
// and valid data packet. Scroll down to the function validPacketFoundHandler
// to see how to process and extract the values from the packet.
pf.processReceivedData(buffer, numOfBytes);
// Processing received asynchronous data from the sensor is fairly straight
// forward. However, you may wish to query or configure the sensor and in
// theory this is still straight forward if you do not add code to handle
// retransmits of commands or communication timeouts. We will show the
// basic structure of querying and configuring and leave it to the
// developer for adding edge case handling code. The file sensors.cpp may
// serve as a reference for adding this extra code.
// We will first illustrate querying the sensor's model number. First we
// generate a read register command.
numOfBytes = Packet::genReadModelNumber(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer));
// Now send the data to the sensor.
gIsCheckingForModelNumberResponse = true;
UserUart_sendData(buffer, numOfBytes);
// Mock that the sensor responded to our request.
UserUart_mockReceivedData("$VNRRG,01,VN-200T-CR*31\r\n");
// Now process the mock data that our fake UART port received and hand it
// over to our UART packet finder.
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
gIsCheckingForModelNumberResponse = false;
// Let's see how to perform a trivial configuration of the sensor. We will
// change the asynchronous data output frequency to 2 Hz.
numOfBytes = Packet::genWriteAsyncDataOutputFrequency(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer), 2);
// Now send the data to the sensor.
gIsCheckingForAsyncOutputFreqResponse = true;
UserUart_sendData(buffer, numOfBytes);
// Mock that the sensor responded to our request.
UserUart_mockReceivedData("$VNWRG,07,2*6F\r\n");
// Now process the mock data that our fake UART port received and hand it
// over to our UART packet finder.
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
gIsCheckingForAsyncOutputFreqResponse = false;
// Other configuration registers on the VectorNav sensors have multiple
// fields that need to be set when we write to them. If we only are
// interested in one field, a safe and easy way to perform this is to first
// read the current configuration, change the fields we are concerned with,
// and then write the settings back to the sensor. We will illustrate this
// now by changing the sensors heading mode of the VPE Basic Control
// register.
cout << "Reading current values of the VPE Basic Control register." << endl;
// First generate a read register command.
numOfBytes = Packet::genReadVpeBasicControl(ERRORDETECTIONMODE_CHECKSUM, buffer, sizeof(buffer));
// Now send the data to the sensor.
gIsCheckingForVpeBasicControlResponse = true;
UserUart_sendData(buffer, numOfBytes);
// Mock that the sensor responded to our request.
UserUart_mockReceivedData("$VNRRG,35,1,1,1,1*75\r\n");
// Now process the mock data that our fake UART port received and hand it
// over to our UART packet finder.
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
gIsCheckingForVpeBasicControlResponse = false;
// The validPacketFoundHandler will have set the current values of the VPE
// Basic Control register into our global variables. Let's now change the
// heading mode field for this register while keeping the other fields in
// their current state.
cout << "Writing new values to the VPE Basic Control register." << endl;
// Generate the write register command.
numOfBytes = Packet::genWriteVpeBasicControl(
ERRORDETECTIONMODE_CHECKSUM,
buffer,
sizeof(buffer),
gEnable,
0, // Could possibly use a value from the enum HeadingMode in sensors.h.
gFilteringMode,
gTuningMode);
// Send the data to the sensor.
gIsCheckingForVpeBasicControlResponse = true;
UserUart_sendData(buffer, numOfBytes);
// Mock that the sensor responded to our request.
UserUart_mockReceivedData("$VNWRG,35,1,0,1,1*71\r\n");
// Process the mock data that our fake UART port received and hand it
// over to our UART packet finder.
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
gIsCheckingForVpeBasicControlResponse = false;
// The VectorNav sensor also supports binary asynchronous data output,
// which can be configured by the user to support flexible configuration of
// data output types. In this example, we will show how to configure the
// sensor's binary output configuration register, and then process a packet
// received of this binary output data.
// Generate our command to configure the Binary Output 1 register. Normally
// when working with the functions from the file uart.h, the data types as
// listed in the user manual are used, without any abstractions getting in
// the way. However, here we use some enums defined in sensors.h for
// specifying the flags of the register's fields since it is much easier to
// understand. Here we configure the sensor to output yaw, pitch, roll and
// timestart data at 4 Hz. Note that the sensor's user manual requires
// specifying which groups are present; however, this function call will
// take care of determining which fields are present.
numOfBytes = Packet::genWriteBinaryOutput1(
ERRORDETECTIONMODE_CHECKSUM,
buffer,
sizeof(buffer),
ASYNCMODE_PORT1,
200,
COMMONGROUP_TIMESTARTUP | COMMONGROUP_YAWPITCHROLL, // Note use of binary OR to configure flags.
TIMEGROUP_NONE,
IMUGROUP_NONE,
GPSGROUP_NONE,
ATTITUDEGROUP_NONE,
INSGROUP_NONE,
GPSGROUP_NONE);
// Send the data to the sensor.
UserUart_sendData(buffer, numOfBytes);
// Mock that the sensor responded to our request by outputting async binary data.
UserUart_mockReceivedData(reinterpret_cast<char*>(mockBinaryAsyncData), sizeof(mockBinaryAsyncData));
// Process the mock data that our fake UART port received and hand it
// over to our UART packet finder.
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
// Lastly, you may want to include code that checks for error messages
// output from the sensor. To demonstrate, we pass a fake error message to
// be handled by our code.
UserUart_mockReceivedData("$VNERR,12*72\r\n");
UserUart_checkForReceivedData(buffer, sizeof(buffer), &numOfBytes);
pf.processReceivedData(buffer, numOfBytes);
return 0;
}
void validPacketFoundHandler(void* userData, Packet &packet, size_t runningIndexOfPacketStart)
{
// When this function is called, the packet will already have been
// validated so no checksum/CRC check is required.
// First see if this is an ASCII or binary packet.
if (packet.type() == Packet::TYPE_ASCII)
{
// Now that we know this is an ASCII packet, we can call the various
// ASCII functions to further process this packet.
if (packet.isAsciiAsync())
{
// We know we have an ASCII asynchronous data packet. Let's see if
// this is a message type we are looking for.
AsciiAsync asyncType = packet.determineAsciiAsyncType();
if (asyncType == VNYMR)
{
// Parse the VNYMR message.
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;
}
}
else if (packet.isResponse())
{
if (gIsCheckingForModelNumberResponse)
{
char modelNumber[100];
packet.parseModelNumber(modelNumber);
cout << "Model Number: " << modelNumber << endl;
}
else if (gIsCheckingForAsyncOutputFreqResponse)
{
uint32_t asyncOutputFreq;
packet.parseAsyncDataOutputFrequency(&asyncOutputFreq);
cout << "Asynchronous Output Frequency: " << asyncOutputFreq << " Hz" << endl;
}
else if (gIsCheckingForVpeBasicControlResponse)
{
packet.parseVpeBasicControl(&gEnable, &gHeadingMode, &gFilteringMode, &gTuningMode);
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;
}
}
else if (packet.isError())
{
cout << "Sensor Error: " << packet.parseError() << endl;
}
}
else if (packet.type() == Packet::TYPE_BINARY)
{
uint64_t timeStartup;
vec3f ypr;
// See if this is a binary packet type we are expecting.
if (!packet.isCompatible(
COMMONGROUP_TIMESTARTUP | COMMONGROUP_YAWPITCHROLL,
TIMEGROUP_NONE,
IMUGROUP_NONE,
GPSGROUP_NONE,
ATTITUDEGROUP_NONE,
INSGROUP_NONE,
GPSGROUP_NONE))
{
// Not the type of binary packet we are expecting.
return;
}
// Ok, we have our expected binary output packet. Since there are many
// ways to configure the binary data output, the burden is on the user
// to correctly parse the binary packet. However, we can make use of
// the parsing convenience methods provided by the VnUartPacket structure.
// When using these convenience methods, you have to extract them in
// the order they are organized in the binary packet per the User Manual.
timeStartup = packet.extractUint64();
ypr = packet.extractVec3f();
cout << "[Binary Packet Received]" << endl;
cout << " TimeStartup: " << timeStartup << endl;
cout << " Yaw Pitch Roll: " << ypr << endl;
}
}
// Some variables to enable our mock UART port.
char* mockUartReceivedDataBuffer[256];
size_t mockUartReceivedDataSize;
// This is a mock function which in a real development environment would
// contain code to initialize the device's UART port. However, to keep this
// example generic, we simply initialize our program to mimic a UART port
// provided by the user.
void UserUart_initialize()
{
mockUartReceivedDataSize = 0;
}
// This is a helper method for our mocked UART port that will mimic data that
// has been received on a UART port. This function would not be implemented in
// an environment that is using an actual UART port.
void UserUart_mockReceivedData(char* buffer, size_t bufferSize)
{
memcpy(mockUartReceivedDataBuffer, buffer, bufferSize);
mockUartReceivedDataSize = bufferSize;
}
// Convenient override of above method.
void UserUart_mockReceivedData(string data)
{
memcpy(mockUartReceivedDataBuffer, data.c_str(), data.length());
mockUartReceivedDataSize = data.length();
}
// This is another mock function which would be replaced in a real program to
// actually query the environment's UART for any data received. We just mock
// the data in this example.
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;
}
// This is a method for simulating sending data to a VectorNav sensor. This
// will need to be implemented by the developer to actually send the data over
// the system's UART.
void UserUart_sendData(char *data, size_t size)
{
// Do nothing since we are mocking a UART port.
}