sensor/main.c

This example illustrates using the high-level VnSensor structure to access a VectorNav sensor on a system with an operating system. This example is in contrast with the getting_started/main.c example which is lower-level protocol handling and requires the user to supply external communication functions.

# Visual Studio (Windows)

1. Open the solution file for your specific Visual Studio version located at `<root>/c/examples/sensor/projects/vs20XX/sensor.sln`.
2. Open the project file `main.c` and edit the `SENSOR_PORT` and `SENSOR_BAUDRATE` constants at the top of the `main()` function to the settings used by your attached VectorNav sensor.
3. Build the entire solution by going to the menu `BUILD -> Build Solution`.
4. Right-click the project `sensor` and select `Debug -> Start new instance`.

# Make (Linux/Mac OS X)

1. Open a terminal and change to the directory `<root>/c/examples/sensor` .
2. Open the file `main.c` and edit the `SENSOR_PORT` and `SENSOR_BAUDRATE` constants at the top of the `main()` function to the settings used by your attached VectorNav sensor.
3. To build the example, run the command `make` .
4. Run the example by executing the command `sudo ./sensor` .

# CLion (Windows/Linux/Mac OS X)

1. Open the project file located at `<root>/c/examples/sensor` in CLion.
2. Open the project file `main.c` and edit the `SENSOR_PORT` and `SENSOR_BAUDRATE` constants at the top of the `main()` function to the settings used by your attached VectorNav sensor.
3. Make sure the `sensor` configuration is active. You can set this by clicking the small drop-down list in the upper-right corner and selecting the option `sensor`.
4. Build the solution by going to the menu `Run -> Build`.
5. Run the example by going to the menu `Run -> Run 'sensor'`.

# Other

To compile and run for an environment not listed here, you will need to add all of the `*.c` files in the directory `<root>/c/src` along with the file located at `<root>/c/examples/sensor/main.c` to your project for compilation. You will also need to add `<root>/c/include` to your include directories. Finally, before compiling, open the file `main.c` and edit the `SENSOR_PORT` and `SENSOR_BAUDRATE` constants at the top of the `main()` function to the settings used by your attached VectorNav sensor.

#include <stdio.h>
#include <inttypes.h>
/* Include files needed to use VnSensor. */
#include "vn/sensors.h"
void asciiAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex);
void asciiOrBinaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex);
int processErrorReceived(char* errorMessage, VnError errorCode);
int main(void)
{
VnSensor vs;
char modelNumber[30];
char strConversions[50];
vec3f ypr;
YawPitchRollMagneticAccelerationAndAngularRatesRegister reg;
VpeBasicControlRegister vpeReg;
uint32_t oldHz, newHz;
VnAsciiAsync asyncType;
BinaryOutputRegister bor;
VnError error;
/* This example walks through using the VectorNav C Library to connect to
* and interact with a VectorNav sensor using the VnSensor structure. */
/* First determine which COM port your sensor is attached to and update the
* constant below. Also, if you have changed your sensor from the factory
* default baudrate of 115200, you will need to update the baudrate
* constant below as well. */
const char SENSOR_PORT[] = "COM1"; /* Windows format for physical and virtual (USB) serial port. */
/*const char SENSOR_PORT[] = "/dev/ttyS1"; */ /* Linux format for physical serial port. */
/*const char SENSOR_PORT[] = "/dev/ttyUSB0"; */ /* Linux format for virtual (USB) serial port. */
/*const char SENSOR_PORT[] = "/dev/tty.usbserial-FTXXXXXX"; */ /* Mac OS X format for virtual (USB) serial port. */
/*const char SENSOR_PORT[] = "/dev/ttyS0"; */ /* CYGWIN format. Usually the Windows COM port number minus 1. This would connect to COM1. */
const uint32_t SENSOR_BAUDRATE = 115200;
/* We first need to initialize our VnSensor structure. */
VnSensor_initialize(&vs);
/* Now connect to our sensor. */
if ((error = VnSensor_connect(&vs, SENSOR_PORT, SENSOR_BAUDRATE)) != E_NONE)
return processErrorReceived("Error connecting to sensor.", error);
/* Let's query the sensor's model number. */
if ((error = VnSensor_readModelNumber(&vs, modelNumber, sizeof(modelNumber))) != E_NONE)
return processErrorReceived("Error reading model number.", error);
printf("Model Number: %s\n", modelNumber);
/* Get some orientation data from the sensor. */
if ((error = VnSensor_readYawPitchRoll(&vs, &ypr)) != E_NONE)
return processErrorReceived("Error reading yaw pitch roll.", error);
str_vec3f(strConversions, ypr);
printf("Current YPR: %s\n", strConversions);
/* Get some orientation and IMU data. */
if ((error = VnSensor_readYawPitchRollMagneticAccelerationAndAngularRates(&vs, &reg)) != E_NONE)
return processErrorReceived("Error reading orientation and IMU data.", error);
str_vec3f(strConversions, reg.yawPitchRoll);
printf("Current YPR: %s\n", strConversions);
str_vec3f(strConversions, reg.mag);
printf("Current Magnetic: %s\n", strConversions);
str_vec3f(strConversions, reg.accel);
printf("Current Acceleration: %s\n", strConversions);
str_vec3f(strConversions, reg.gyro);
printf("Current Angular Rates: %s\n", strConversions);
/* Let's do some simple reconfiguration of the sensor. As it comes from the
* factory, the sensor outputs asynchronous data at 40 Hz. We will change
* this to 2 Hz for demonstration purposes. */
if ((error = VnSensor_readAsyncDataOutputFrequency(&vs, &oldHz)) != E_NONE)
return processErrorReceived("Error reading async data output frequency.", error);
if ((error = VnSensor_writeAsyncDataOutputFrequency(&vs, 2, true)) != E_NONE)
return processErrorReceived("Error writing async data output frequency.", error);
if ((error = VnSensor_readAsyncDataOutputFrequency(&vs, &newHz)) != E_NONE)
return processErrorReceived("Error reading async data output frequency.", error);
printf("Old Async Frequency: %d Hz\n", oldHz);
printf("New Async Frequency: %d Hz\n", newHz);
/* For the registers that have more complex configuration options, it is
* convenient to read the current existing register configuration, change
* only the values of interest, and then write the configuration to the
* register. This allows preserving the current settings for the register's
* other fields. Below, we change the heading mode used by the sensor. */
if ((error = VnSensor_readVpeBasicControl(&vs, &vpeReg)) != E_NONE)
return processErrorReceived("Error reading VPE basic control.", error);
strFromHeadingMode(strConversions, vpeReg.headingMode);
printf("Old Heading Mode: %s\n", strConversions);
vpeReg.headingMode = VNHEADINGMODE_ABSOLUTE;
if ((error = VnSensor_writeVpeBasicControl(&vs, vpeReg, true)) != E_NONE)
return processErrorReceived("Error writing VPE basic control.", error);
if ((error = VnSensor_readVpeBasicControl(&vs, &vpeReg)) != E_NONE)
return processErrorReceived("Error reading VPE basic control.", error);
strFromHeadingMode(strConversions, vpeReg.headingMode);
printf("New Heading Mode: %s\n", strConversions);
/* Up to now, we have shown some examples of how to configure the sensor
* and query for the latest measurements. However, this querying is a
* relatively slow method for getting measurements since the CPU has to
* send out the command to the sensor and also wait for the command
* response. An alternative way of receiving the sensor's latest
* measurements without the waiting for a query response, you can configure
* the library to alert you when new asynchronous data measurements are
* received. We will illustrate hooking up to our current VnSensor to
* receive these notifications of asynchronous messages. */
/* First let's configure the sensor to output a known asynchronous data
* message type. */
if ((error = VnSensor_writeAsyncDataOutputType(&vs, VNYPR, true)) != E_NONE)
return processErrorReceived("Error writing to async data output type.", error);
if ((error = VnSensor_readAsyncDataOutputType(&vs, &asyncType)) != E_NONE)
return processErrorReceived("Error reading async data output type.", error);
strFromVnAsciiAsync(strConversions, asyncType);
printf("ASCII Async Type: %s\n", strConversions);
/* You will need to define a method which has the appropriate
* signature for receiving notifications. This is implemented with the
* method asciiAsyncMessageReceived. Now we register the method with the
* VnSensor structure. */
VnSensor_registerAsyncPacketReceivedHandler(&vs, asciiAsyncMessageReceived, NULL);
/* Now sleep for 5 seconds so that our asynchronous callback method can
* receive and display receive yaw, pitch, roll packets. */
printf("Starting sleep...\n");
VnThread_sleepSec(5);
/* Unregister our callback method. */
VnSensor_unregisterAsyncPacketReceivedHandler(&vs);
/* As an alternative to receiving notifications of new ASCII asynchronous
* messages, the binary output configuration of the sensor is another
* popular choice for receiving data since it is compact, fast to parse,
* and can be output at faster rates over the same connection baudrate.
* Here we will configure the binary output register and process packets
* with a new callback method that can handle both ASCII and binary
* packets. */
/* First we create a structure for setting the configuration information
* for the binary output register to send yaw, pitch, roll data out at
* 4 Hz. */
BinaryOutputRegister_initialize(
&bor,
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);
if ((error = VnSensor_writeBinaryOutput1(&vs, &bor, true)) != E_NONE)
return processErrorReceived("Error writing binary output 1.", error);
VnSensor_registerAsyncPacketReceivedHandler(&vs, asciiOrBinaryAsyncMessageReceived, NULL);
printf("Starting sleep...\n");
VnThread_sleepSec(5);
VnSensor_unregisterAsyncPacketReceivedHandler(&vs);
/* Now disconnect from the sensor since we are finished. */
if ((error = VnSensor_disconnect(&vs)) != E_NONE)
return processErrorReceived("Error disconnecting from sensor.", error);
return 0;
}
/* This is our basic callback handler for notifications of new asynchronous
* data packets received. The userData parameter is a pointer to the data we
* supplied when we called registerAsyncPacketReceivedHandler. In this case
* we didn't need any user data so we just set this to NULL. Alternatively you
* can provide a pointer to user data which you can use in the callback method.
* One use for this is help in calling back to a member method instead of just
* a global or static method. The Packet p parameter is an encapsulation of
* the data packet. At this state, it has already been validated and identified
* as an asynchronous data message. However, some processing is required on the
* user side to make sure it is the right type of asynchronous message type so
* we can parse it correctly. The index parameter is an advanced usage item and
* can be safely ignored for now. */
void asciiAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex)
{
vec3f ypr;
char strConversions[50];
/* Silence 'unreferenced formal parameters' warning in Visual Studio. */
(userData);
(runningIndex);
/* Make sure we have an ASCII packet and not a binary packet. */
if (VnUartPacket_type(packet) != PACKETTYPE_ASCII)
return;
/* Make sure we have a VNYPR data packet. */
if (VnUartPacket_determineAsciiAsyncType(packet) != VNYPR)
return;
/* We now need to parse out the yaw, pitch, roll data. */
VnUartPacket_parseVNYPR(packet, &ypr);
/* Now print out the yaw, pitch, roll measurements. */
str_vec3f(strConversions, ypr);
printf("ASCII Async YPR: %s\n", strConversions);
}
void asciiOrBinaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex)
{
vec3f ypr;
char strConversions[50];
/* Silence 'unreferenced formal parameters' warning in Visual Studio. */
(userData);
(runningIndex);
if (VnUartPacket_type(packet) == PACKETTYPE_ASCII && VnUartPacket_determineAsciiAsyncType(packet) == VNYPR)
{
VnUartPacket_parseVNYPR(packet, &ypr);
str_vec3f(strConversions, ypr);
printf("ASCII Async YPR: %s\n", strConversions);
return;
}
if (VnUartPacket_type(packet) == PACKETTYPE_BINARY)
{
uint64_t timeStartup;
/* First make sure we have a binary packet type we expect since there
* are many types of binary output types that can be configured. */
if (!VnUartPacket_isCompatible(packet,
COMMONGROUP_TIMESTARTUP | COMMONGROUP_YAWPITCHROLL,
TIMEGROUP_NONE,
IMUGROUP_NONE,
GPSGROUP_NONE,
ATTITUDEGROUP_NONE,
INSGROUP_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 Packet 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 = VnUartPacket_extractUint64(packet);
ypr = VnUartPacket_extractVec3f(packet);
str_vec3f(strConversions, ypr);
printf("Binary Async TimeStartup: %" PRIu64 "\n", timeStartup);
printf("Binary Async YPR: %s\n", strConversions);
return;
}
}
int processErrorReceived(char* errorMessage, VnError errorCode)
{
char errorCodeStr[100];
strFromVnError(errorCodeStr, errorCode);
printf("%s\nERROR: %s\n", errorMessage, errorCodeStr);
return -1;
}