#include #include /* 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, ®)) != 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, GPSGROUP_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, 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 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; }