From 9c154b62a06c46b134a68978fa2c3b15a7c409e1 Mon Sep 17 00:00:00 2001 From: DWatkins Date: Wed, 22 Jun 2016 20:18:02 -0700 Subject: [PATCH] added vn-100 library --- src/vectornav/CMakeLists.txt | 24 + src/vectornav/include/vectornav/vectornav.h | 38 + src/vectornav/include/vectornav/vn100.h | 2219 +++++++ src/vectornav/include/vectornav/vn200.h | 1746 ++++++ src/vectornav/include/vectornav/vn_common.h | 44 + .../include/vectornav/vn_errorCodes.h | 71 + .../include/vectornav/vn_kinematics.h | 52 + .../include/vectornav/vn_linearAlgebra.h | 62 + src/vectornav/include/vectornav/vn_math.h | 42 + .../include/vectornav/vncp_services.h | 314 + src/vectornav/include/vectornav/vndevice.h | 1747 ++++++ src/vectornav/include/vectornav/vnint.h | 43 + src/vectornav/package.xml | 19 + src/vectornav/src/vectornav/vn100.c | 3133 +++++++++ src/vectornav/src/vectornav/vncp_services.c | 424 ++ src/vectornav/src/vectornav/vndevice.c | 5581 +++++++++++++++++ 16 files changed, 15559 insertions(+) create mode 100644 src/vectornav/CMakeLists.txt create mode 100755 src/vectornav/include/vectornav/vectornav.h create mode 100755 src/vectornav/include/vectornav/vn100.h create mode 100755 src/vectornav/include/vectornav/vn200.h create mode 100755 src/vectornav/include/vectornav/vn_common.h create mode 100755 src/vectornav/include/vectornav/vn_errorCodes.h create mode 100755 src/vectornav/include/vectornav/vn_kinematics.h create mode 100755 src/vectornav/include/vectornav/vn_linearAlgebra.h create mode 100755 src/vectornav/include/vectornav/vn_math.h create mode 100755 src/vectornav/include/vectornav/vncp_services.h create mode 100755 src/vectornav/include/vectornav/vndevice.h create mode 100755 src/vectornav/include/vectornav/vnint.h create mode 100644 src/vectornav/package.xml create mode 100755 src/vectornav/src/vectornav/vn100.c create mode 100755 src/vectornav/src/vectornav/vncp_services.c create mode 100755 src/vectornav/src/vectornav/vndevice.c diff --git a/src/vectornav/CMakeLists.txt b/src/vectornav/CMakeLists.txt new file mode 100644 index 0000000..2ae49cb --- /dev/null +++ b/src/vectornav/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) +project(vectornav) + +## Start Global Marker + +## End Global Marker + +## Check C++11 / C++0x +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "-std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "-std=c++0x") +else() + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +include_directories(include/vectornav) +add_library(vectornav + src/vectornav/vn100.c + src/vectornav/vncp_services.c + src/vectornav/vndevice.c) diff --git a/src/vectornav/include/vectornav/vectornav.h b/src/vectornav/include/vectornav/vectornav.h new file mode 100755 index 0000000..0b31346 --- /dev/null +++ b/src/vectornav/include/vectornav/vectornav.h @@ -0,0 +1,38 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file includes all of the header file inclusions needed to use + * all of the features of the VectorNav C/C++ Library. + */ +#ifndef _VECTORNAV_H_ +#define _VECTORNAV_H_ + +#include "vn100.h" +#include "vn200.h" +#include "vn_math.h" + +#endif /* VECTORNAV_H */ diff --git a/src/vectornav/include/vectornav/vn100.h b/src/vectornav/include/vectornav/vn100.h new file mode 100755 index 0000000..80a64f9 --- /dev/null +++ b/src/vectornav/include/vectornav/vn100.h @@ -0,0 +1,2219 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to devices based on VectorNav's VN-100 + * family of orientation sensors. + */ +#ifndef _VN100_H_ +#define _VN100_H_ + +/* Disable some unecessary warnings for compiling using Visual Studio with -Wall. */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4668) /* Preprocessor macro not defined warning. */ +#endif + +#if defined(_MSC_VER) && _MSC_VER <= 1500 + /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ + #include "vnint.h" +#else + #include +#endif + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +#include "vndevice.h" +#include "vncp_services.h" +#include "vn_kinematics.h" +#include "vn_linearAlgebra.h" + +#if defined(EXPORT_TO_DLL) + #define DLL_EXPORT __declspec(dllexport) +#else + /** Don't compile the library with support for DLL function export. */ + #define DLL_EXPORT +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* Disable some unecessary warnings for compiling using Visual Studio with -Wall. */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4820) /* Padding added in structures warning. */ +#endif + +/** + * \brief Holds connection information for accessing a VN-100 device. + */ +typedef struct { + char* portName; /**< The name of the serial port. */ + int baudRate; /**< The baudrate of the serial port. */ + bool isConnected; /**< Inidicates if the serial port is open. */ + VnDevice vndevice; /**< Pointer to internally used data. */ +} Vn100; + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +/** + * \brief Connects to a VectorNav VN-100 device. + * + * \param[out] newVn100 An uninitialized Vn100 control object should be passed in. + * \param[in] portName The name of the COM port to connect to. + * \param[in] baudrate The baudrate to connect at. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_connect( + Vn100* newVn100, + const char* portName, + int baudrate); + +/** + * \brief Disconnects from the VN-100 device and disposes of any internal resources. + * + * \param vn100 Pointer to the Vn100 control object. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_disconnect( + Vn100* vn100); + +/** + * \brief Commands the VN-100 module to zero out its current orientation. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_tare( + Vn100* vn100, + bool waitForResponse); + +/** + * \brief Notifies the VN-100 module if a known magnetic disturbance is present. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] isDisturbancePresent True if a known magnetic disturbance is present. False if not. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_knownMagneticDisturbance( + Vn100* vn100, + bool isDisturbancePresent, + bool waitForResponse); + +/** + * \brief Notifies the VN-100 module if a known acceleration disturbance is present. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] isDisturbancePresent True if a known acceleration disturbance is present. False if not. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_knownAccelerationDisturbance( + Vn100* vn100, + bool isDisturbancePresent, + bool waitForResponse); + +/** + * \brief Commands the VN-100 module to save the current gyro bias estimate to + * memory for use on the next module startup. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setGyroBias( + Vn100* vn100, + bool waitForResponse); + +/** + * \brief Allows registering a function which will be called whenever a new + * asynchronous data packet is received from the VN-100 module. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] listener The function pointer to be called when asynchronous data + * is received. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_registerAsyncDataReceivedListener( + Vn100* vn100, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when new asynchronous data is received. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_unregisterAsyncDataReceivedListener( + Vn100* vn100, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Allows registering a function which will be called whenever an error + * code is received from the VN-100 module. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] listener The function pointer to be called when an error code + * is received. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vn100_registerErrorCodeReceivedListener( + Vn100* vn100, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when error codes from the VN-100 module are received. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vn100_unregisterErrorCodeReceivedListener( + Vn100* vn100, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Checks if we are able to send and receive communication with the VN-100 sensor. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * + * \return VN_TRUE if the library was able to send and receive a valid response from the VN-100 sensor; otherwise VN_FALSE. + */ +DLL_EXPORT bool vn100_verifyConnectivity( + Vn100* vn100); + +/** + * \brief Gets the current configuration of the requested binary output register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to query for its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getBinaryOutputConfiguration( + Vn100* vn100, + uint8_t binaryOutputRegisterId, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections); + +/** + * \brief Gets the current configuration of the binary output register 1. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getBinaryOutput1Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections); + +/** + * \brief Gets the current configuration of the binary output register 2. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getBinaryOutput2Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections); + +/** + * \brief Gets the current configuration of the binary output register 3. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getBinaryOutput3Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections); + +/** + * \brief Sets the configuration of the requested binary output register. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to set its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setBinaryOutputConfiguration( + Vn100* vn100, + uint8_t binaryOutputRegisterId, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 1. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setBinaryOutput1Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 2. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setBinaryOutput2Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 3. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setBinaryOutput3Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse); + + +/** + * \brief Gets the values in the Quaternion and Magnetic register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternionMagneticAccelerationAngularRate + * instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionMagnetic( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic); + +/** + * \brief Gets the values in the Quaternion Acceleration register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternionMagneticAccelerationAngularRate + * instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionAcceleration( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* acceleration); + +/** + * \brief Gets the values in the Quaternion Angular Rates register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternionMagneticAccelerationAngularRate + * instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Quaternion, Magnetic and Acceleration register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternionMagneticAccelerationAngularRate + * instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionMagneticAcceleration( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration); + +/** + * \brief Gets the values in the Quaternion, Acceleration and Angular Rates register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternionMagneticAccelerationAngularRate + * instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionAccelerationAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Attitude (Directional Cosine Matrix) register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. Please use the function vn100_getQuaternion + * or vn100_getYawPitchRoll instead. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor attitude (DCM) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getDirectionCosineMatrix( + Vn100* vn100, + VnMatrix3x3* attitude); + +/** + * \brief Gets the values in the Filter Measurement Variance Parameters register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] angularWalkVariance The angular walk variance value of the sensor. + * \param[out] angularRateVariance The current sensor angular rate variance (X,Y,Z) values. + * \param[out] magneticVariance The current sensor magnetic variance (X,Y,Z) values. + * \param[out] accelerationVariance The current sensor acceleration variance (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFilterMeasurementVarianceParameters( + Vn100* vn100, + double* angularWalkVariance, + VnVector3* angularRateVariance, + VnVector3* magneticVariance, + VnVector3* accelerationVariance); + +/** + * \brief Sets the values of the Filter Measurement Variance Parameters register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] angularWalkVariance Value for the angular walk variance field. + * \param[in] angularRateVariance The angular rate variance (X,Y,Z) values to write to the sensor. + * \param[in] magneticVariance The magnetic variance (X,Y,Z) values to write to the sensor. + * \param[in] accelerationVariance The acceleration variance (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setFilterMeasurementVarianceParameters( + Vn100* vn100, + double angularWalkVariance, + VnVector3 angularRateVariance, + VnVector3 magneticVariance, + VnVector3 accelerationVariance, + bool waitForResponse); + +/** + * \brief Gets the values in the Filter Active Tuning Parameters register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magneticGain The magnetic disturbance gain value of the sensor. + * \param[out] accelerationGain The acceleration disturbance gain value of the sensor. + * \param[out] magneticMemory The magnetic disturbance memory value of the sensor. + * \param[out] accelerationMemory The acceleration disturbance memory value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFilterActiveTuningParameters( + Vn100* vn100, + double* magneticGain, + double* accelerationGain, + double* magneticMemory, + double* accelerationMemory); + +/** + * \brief Sets the values of the Filter Active Tuning Parameters register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] magneticGain Value for the magnetic disturbance gain field. + * \param[in] accelerationGain Value for the acceleration disturbance gain field. + * \param[in] magneticMemory Value for the magnetic disturbance memory field. + * \param[in] accelerationMemory Value for the acceleration disturbance memory field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setFilterActiveTuningParameters( + Vn100* vn100, + double magneticGain, + double accelerationGain, + double magneticMemory, + double accelerationMemory, + bool waitForResponse); + +/** + * \brief Gets the values in the Accelerometer Gain register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] accelerometerGain The accelerometer gain mode value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAccelerometerGain( + Vn100* vn100, + unsigned int* accelerometerGain); + +/** + * \brief Sets the values of the Accelerometer Gain register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] accelerometerGain Value for the accelerometer gain mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setAccelerometerGain( + Vn100* vn100, + unsigned int accelerometerGain, + bool waitForResponse); + +/** + * \brief Gets the values in the Communication Protocol Status register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] numOfParsedSerialMessages The number of successfully parsed serial messages received value of the sensor. + * \param[out] numOfParsedSpiMessages The number of successfully parsed SPI messages received value of the sensor. + * \param[out] maxUsageSerialRxBuffer The maximum percent usage of serial incoming buffer value of the sensor. + * \param[out] maxUsageSerialTxBuffer The maximum percent usage of serial outgoing buffer value of the sensor. + * \param[out] maxUsageSpiRxBuffer The maximum percent usage of SPI incoming buffer value of the sensor. + * \param[out] maxUsageSpiTxBuffer The maximum percent usage of SPI outgoing buffer value of the sensor. + * \param[out] systemError0 The SystemError 0 value of the sensor. + * \param[out] systemError1 The SystemError 1 value of the sensor. + * \param[out] systemError2 The SystemError 2 value of the sensor. + * \param[out] systemError3 The SystemError 3 value of the sensor. + * \param[out] systemError4 The SystemError 4 value of the sensor. + * \param[out] systemError5 The SystemError 5 value of the sensor. + * \param[out] systemError6 The SystemError 6 value of the sensor. + * \param[out] systemError7 The SystemError 7 value of the sensor. + * \param[out] systemError8 The SystemError 8 value of the sensor. + * \param[out] systemError9 The SystemError 9 value of the sensor. + * \param[out] systemError10 The SystemError 10 value of the sensor. + * \param[out] systemError11 The SystemError 11 value of the sensor. + * \param[out] systemError12 The SystemError 12 value of the sensor. + * \param[out] systemError13 The SystemError 13 value of the sensor. + * \param[out] systemError14 The SystemError 14 value of the sensor. + * \param[out] systemError15 The SystemError 15 value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getCommunicationProtocolStatus( + Vn100* vn100, + unsigned int* numOfParsedSerialMessages, + unsigned int* numOfParsedSpiMessages, + unsigned char* maxUsageSerialRxBuffer, + unsigned char* maxUsageSerialTxBuffer, + unsigned char* maxUsageSpiRxBuffer, + unsigned char* maxUsageSpiTxBuffer, + unsigned short* systemError0, + unsigned short* systemError1, + unsigned short* systemError2, + unsigned short* systemError3, + unsigned short* systemError4, + unsigned short* systemError5, + unsigned short* systemError6, + unsigned short* systemError7, + unsigned short* systemError8, + unsigned short* systemError9, + unsigned short* systemError10, + unsigned short* systemError11, + unsigned short* systemError12, + unsigned short* systemError13, + unsigned short* systemError14, + unsigned short* systemError15); + +/** + * \brief Sets the values of the Communication Protocol Status register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] numOfParsedSerialMessages Value for the number of successfully parsed serial messages received field. + * \param[in] numOfParsedSpiMessages Value for the number of successfully parsed SPI messages received field. + * \param[in] maxUsageSerialRxBuffer Value for the maximum percent usage of serial incoming buffer field. + * \param[in] maxUsageSerialTxBuffer Value for the maximum percent usage of serial outgoing buffer field. + * \param[in] maxUsageSpiRxBuffer Value for the maximum percent usage of SPI incoming buffer field. + * \param[in] maxUsageSpiTxBuffer Value for the maximum percent usage of SPI outgoing buffer field. + * \param[in] systemError0 Value for the SystemError 0 field. + * \param[in] systemError1 Value for the SystemError 1 field. + * \param[in] systemError2 Value for the SystemError 2 field. + * \param[in] systemError3 Value for the SystemError 3 field. + * \param[in] systemError4 Value for the SystemError 4 field. + * \param[in] systemError5 Value for the SystemError 5 field. + * \param[in] systemError6 Value for the SystemError 6 field. + * \param[in] systemError7 Value for the SystemError 7 field. + * \param[in] systemError8 Value for the SystemError 8 field. + * \param[in] systemError9 Value for the SystemError 9 field. + * \param[in] systemError10 Value for the SystemError 10 field. + * \param[in] systemError11 Value for the SystemError 11 field. + * \param[in] systemError12 Value for the SystemError 12 field. + * \param[in] systemError13 Value for the SystemError 13 field. + * \param[in] systemError14 Value for the SystemError 14 field. + * \param[in] systemError15 Value for the SystemError 15 field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setCommunicationProtocolStatus( + Vn100* vn100, + unsigned int numOfParsedSerialMessages, + unsigned int numOfParsedSpiMessages, + unsigned char maxUsageSerialRxBuffer, + unsigned char maxUsageSerialTxBuffer, + unsigned char maxUsageSpiRxBuffer, + unsigned char maxUsageSpiTxBuffer, + unsigned short systemError0, + unsigned short systemError1, + unsigned short systemError2, + unsigned short systemError3, + unsigned short systemError4, + unsigned short systemError5, + unsigned short systemError6, + unsigned short systemError7, + unsigned short systemError8, + unsigned short systemError9, + unsigned short systemError10, + unsigned short systemError11, + unsigned short systemError12, + unsigned short systemError13, + unsigned short systemError14, + unsigned short systemError15, + bool waitForResponse); + +/** + * \brief Gets the values in the Filter Basic Control register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magneticMode The magnetic mode value of the sensor. + * \param[out] externalMagnetometerMode The external magnetometer mode value of the sensor. + * \param[out] externalAccelerometerMode The external accelerometer mode value of the sensor. + * \param[out] externalGyroscopeMode The external gyroscope mode value of the sensor. + * \param[out] angularRateLimit The current sensor angular rate saturation liimit (X,Y,Z) values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFilterBasicControl( + Vn100* vn100, + unsigned char* magneticMode, + unsigned char* externalMagnetometerMode, + unsigned char* externalAccelerometerMode, + unsigned char* externalGyroscopeMode, + VnVector3* angularRateLimit); + +/** + * \brief Sets the values of the Filter Basic Control register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] magneticMode Value for the magnetic mode field. + * \param[in] externalMagnetometerMode Value for the external magnetometer mode field. + * \param[in] externalAccelerometerMode Value for the external accelerometer mode field. + * \param[in] externalGyroscopeMode Value for the external gyroscope mode field. + * \param[in] angularRateLimit The angular rate saturation liimit (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setFilterBasicControl( + Vn100* vn100, + unsigned char magneticMode, + unsigned char externalMagnetometerMode, + unsigned char externalAccelerometerMode, + unsigned char externalGyroscopeMode, + VnVector3 angularRateLimit, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Magnetometer Advanced Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] minimumFiltering The current sensor minimum allowed level of filtering (X,Y,Z) values. + * \param[out] maximumFiltering The current sensor maximum allowed level of filtering (X,Y,Z) values. + * \param[out] maximumAdaptRate The MaxAdaptRate value of the sensor. + * \param[out] disturbanceWindow The DisturbanceWindow value of the sensor. + * \param[out] maximumTuning The MaxTuning value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeMagnetometerAdvancedTuning( + Vn100* vn100, + VnVector3* minimumFiltering, + VnVector3* maximumFiltering, + float* maximumAdaptRate, + float* disturbanceWindow, + float* maximumTuning); + +/** + * \brief Sets the values of the VPE Magnetometer Advanced Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] minimumFiltering The minimum allowed level of filtering (X,Y,Z) values to write to the sensor. + * \param[in] maximumFiltering The maximum allowed level of filtering (X,Y,Z) values to write to the sensor. + * \param[in] maximumAdaptRate Value for the MaxAdaptRate field. + * \param[in] disturbanceWindow Value for the DisturbanceWindow field. + * \param[in] maximumTuning Value for the MaxTuning field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeMagnetometerAdvancedTuning( + Vn100* vn100, + VnVector3 minimumFiltering, + VnVector3 maximumFiltering, + float maximumAdaptRate, + float disturbanceWindow, + float maximumTuning, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Accelerometer Advanced Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] minimumFiltering The current sensor minimum allowed level of filtering (X,Y,Z) values. + * \param[out] maximumFiltering The current sensor maximum allowed level of filtering (X,Y,Z) values. + * \param[out] maximumAdaptRate The MaxAdaptRate value of the sensor. + * \param[out] disturbanceWindow The DisturbanceWindow value of the sensor. + * \param[out] maximumTuning The MaxTuning value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeAccelerometerAdvancedTuning( + Vn100* vn100, + VnVector3* minimumFiltering, + VnVector3* maximumFiltering, + float* maximumAdaptRate, + float* disturbanceWindow, + float* maximumTuning); + +/** + * \brief Sets the values of the VPE Accelerometer Advanced Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] minimumFiltering The minimum allowed level of filtering (X,Y,Z) values to write to the sensor. + * \param[in] maximumFiltering The maximum allowed level of filtering (X,Y,Z) values to write to the sensor. + * \param[in] maximumAdaptRate Value for the MaxAdaptRate field. + * \param[in] disturbanceWindow Value for the DisturbanceWindow field. + * \param[in] maximumTuning Value for the MaxTuning field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeAccelerometerAdvancedTuning( + Vn100* vn100, + VnVector3 minimumFiltering, + VnVector3 maximumFiltering, + float maximumAdaptRate, + float disturbanceWindow, + float maximumTuning, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Gyro Basic Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] varianceAngularWalk The current sensor gyroscope angular walk variance (X,Y,Z) values. + * \param[out] baseTuning The current sensor gyroscope base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor gyroscope adaptive tuning (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeGyroBasicTuning( + Vn100* vn100, + VnVector3* varianceAngularWalk, + VnVector3* baseTuning, + VnVector3* adaptiveTuning); + +/** + * \brief Sets the values of the VPE Gyro Basic Tuning register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] varianceAngularWalk The gyroscope angular walk variance (X,Y,Z) values to write to the sensor. + * \param[in] baseTuning The gyroscope base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The gyroscope adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeGyroBasicTuning( + Vn100* vn100, + VnVector3 varianceAngularWalk, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + bool waitForResponse); + +/** + * \brief Gets the values in the Filter Status register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] solutionStatus The solution status bitfield value of the sensor. + * \param[out] yawUncertainty The YawUncertainty value of the sensor. + * \param[out] pitchUncertainty The PitchUncertainty value of the sensor. + * \param[out] rollUncertainty The RollUncertainty value of the sensor. + * \param[out] gyroBiasUncertainty The GyroBiasUncertainty value of the sensor. + * \param[out] magUncertainty The MagUncertainty value of the sensor. + * \param[out] accelUncertainty The AccelUncertainty value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFilterStatus( + Vn100* vn100, + unsigned short* solutionStatus, + float* yawUncertainty, + float* pitchUncertainty, + float* rollUncertainty, + float* gyroBiasUncertainty, + float* magUncertainty, + float* accelUncertainty); + +/** + * \brief Gets the values in the Filter Startup Gyro Bias register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] gyroBias The current sensor gyroscope startup bias (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFilterStartupGyroBias( + Vn100* vn100, + VnVector3* gyroBias); + +/** + * \brief Sets the values of the Filter Startup Gyro Bias register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] gyroBias The gyroscope startup bias (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setFilterStartupGyroBias( + Vn100* vn100, + VnVector3 gyroBias, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetometer Calibration Status register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] lastBin The LastBin value of the sensor. + * \param[out] numOfMeasurements The NumMeas value of the sensor. + * \param[out] avgResidual The AvgResidual value of the sensor. + * \param[out] lastMeasurement The current sensor last measurement (X,Y,Z) values. + * \param[out] bin0 The number of measurements in bin 1 value of the sensor. + * \param[out] bin1 The number of measurements in bin 2 value of the sensor. + * \param[out] bin2 The number of measurements in bin 3 value of the sensor. + * \param[out] bin3 The number of measurements in bin 4 value of the sensor. + * \param[out] bin4 The number of measurements in bin 5 value of the sensor. + * \param[out] bin5 The number of measurements in bin 6 value of the sensor. + * \param[out] bin6 The number of measurements in bin 7 value of the sensor. + * \param[out] bin7 The number of measurements in bin 8 value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagnetometerCalibrationStatus( + Vn100* vn100, + unsigned char* lastBin, + unsigned short* numOfMeasurements, + float* avgResidual, + VnVector3* lastMeasurement, + unsigned char* bin0, + unsigned char* bin1, + unsigned char* bin2, + unsigned char* bin3, + unsigned char* bin4, + unsigned char* bin5, + unsigned char* bin6, + unsigned char* bin7); + +/** + * \brief Gets the values in the Indoor Heading Mode Control register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] maxRateError The MaxRateError value of the sensor. + * \param[out] reserved The reserved value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getIndoorHeadingModeControl( + Vn100* vn100, + float* maxRateError, + float* reserved); + +/** + * \brief Sets the values of the Indoor Heading Mode Control register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] maxRateError Value for the MaxRateError field. + * \param[in] reserved Value for the reserved field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setIndoorHeadingModeControl( + Vn100* vn100, + float maxRateError, + float reserved, + bool waitForResponse); + +/** + * \brief Gets the values in the Velocity Compenstation Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] mode The Mode field. + * \param[out] velocityTuning The VelocityTuning field. + * \param[out] rateTuning The RateTuning field. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVelocityCompenstationControl( + Vn100* vn100, + uint8_t* mode, + float* velocityTuning, + float* rateTuning); + +/** + * \brief Sets the values of the Velocity Compenstation Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] mode The Mode field. + * \param[in] velocityTuning The VelocityTuning field. + * \param[in] rateTuning The RateTuning field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVelocityCompenstationControl( + Vn100* vn100, + uint8_t mode, + float velocityTuning, + float rateTuning, + bool waitForResponse); + +/** + * \brief Gets the values in the Velocity Compenstation Measurement register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] velocity The Velocity field. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVelocityCompenstationMeasurement( + Vn100* vn100, + VnVector3* velocity); + +/** + * \brief Sets the values of the Velocity Compenstation Measurement register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] velocity The Velocity field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVelocityCompenstationMeasurement( + Vn100* vn100, + VnVector3 velocity, + bool waitForResponse); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll and Inertial Calibrated Measurements register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] inertialMagnetic The current sensor inertial magnetic (X,Y,Z) values. + * \param[out] inertialAcceleration The current sensor inertial acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getYawPitchRollInertialCalibratedMeasurements( + Vn100* vn100, + VnYpr* attitude, + VnVector3* inertialMagnetic, + VnVector3* inertialAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Raw Voltage Measurements register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magnetometer The current sensor magnetometer raw voltages (X,Y,Z) values. + * \param[out] accelerometer The current sensor accelerometer raw voltages (X,Y,Z) values. + * \param[out] gyroscope The current sensor gyroscope raw voltages (X,Y,Z) values. + * \param[out] temperature The temperature raw voltages value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getRawVoltageMeasurements( + Vn100* vn100, + VnVector3* magnetometer, + VnVector3* accelerometer, + VnVector3* gyroscope, + float* temperature); + +/** + * \brief Gets the values in the Kalman Filter State Vector register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] gyroscopeBias The current sensor gyroscope bias (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getKalmanFilterStateVector( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* gyroscopeBias); + +/** + * \brief Gets the values in the Kalman Filter Covariance Matrix Diagonal register. + * \deprecated This function is deprecated in version 2.0 and greater of the + * VN-100 firmware. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] p00 The P[0,0] value of the sensor. + * \param[out] p11 The P[1,1] value of the sensor. + * \param[out] p22 The P[2,2] value of the sensor. + * \param[out] p33 The P[3,3] value of the sensor. + * \param[out] p44 The P[4,4] value of the sensor. + * \param[out] p55 The P[5,5] value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getKalmanFilterCovarianceMatrixDiagonal( + Vn100* vn100, + float* p00, + float* p11, + float* p22, + float* p33, + float* p44, + float* p55); + +/** + * \brief Retrieves the associated timeout value for the Vn100 object. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * + * \return The timeout value in milliseconds. -1 indicates that timeouts are + * not used. + */ +DLL_EXPORT int vn100_get_timeout( + Vn100* vn100); + +/** + * \brief Sets the timeout value for the reading values from the VectorNav sensor. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] timeout The timeout value in milliseconds. Specify -1 to not use + * any timeouts. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_set_timeout( + Vn100* vn100, + int timeout); + +/** + * \brief Retrieves the most recent stored asynchronous data. + * + * \param[in] vn100 Pointer to the Vn100 object. + * \param[out] curData Returned pointer current asychronous data. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getCurrentAsyncData( + Vn100* vn100, + VnDeviceCompositeData* curData); + +/** + * \brief Commands the VectorNav unit to write its current register setting to + * non-volatile memory. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_writeSettings( + Vn100* vn100, + bool waitForResponse); + +/** + * \brief Commands the VectorNav unit to revert its settings to factory defaults. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_restoreFactorySettings( + Vn100* vn100, + bool waitForResponse); + +/** + * \brief Commands the VectorNav module to reset itself. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_reset( + Vn100* vn100); + +/** + * \brief Gets the values in the User Tag register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] userTagBuffer Buffer to store the response. Must have a length of at least 21 characters. + * \param[in] userTagBufferLength Length of the provided userTagBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getUserTag( + Vn100* vn100, + char* userTagBuffer, + uint32_t userTagBufferLength); + +/** + * \brief Sets the values of the User Tag register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] userTagData Array containg the data to send. Length must be equal to or less than 20 characters. + * \param[in] userTagDataLength Length of the data to send in the userTagData array. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setUserTag( + Vn100* vn100, + char* userTagData, + uint32_t userTagDataLength, + bool waitForResponse); + +/** + * \brief Gets the values in the Model Number register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] modelBuffer Buffer to store the response. Must have a length of at least 25 characters. + * \param[in] modelBufferLength Length of the provided modelBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getModelNumber( + Vn100* vn100, + char* modelBuffer, + uint32_t modelBufferLength); + +/** + * \brief Gets the values in the Hardware Revision register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] hardwareRevision The hardware revision value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getHardwareRevision( + Vn100* vn100, + int32_t* hardwareRevision); + +/** + * \brief Gets the values in the Serial Number register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] serialNumberBuffer Buffer to store the response. Must have a length of at least 13 characters. + * \param[in] serialNumberBufferLength Length of the provided serialNumberBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getSerialNumber( + Vn100* vn100, + char* serialNumberBuffer, + uint32_t serialNumberBufferLength); + +/** + * \brief Gets the value in the Firmware Version register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] firmwareVersionBuffer Buffer to store the response. Must have a length of at least 16 characters. + * \param[in] firmwareVersionBufferLength Length of the provided firmwareVersionBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getFirmwareVersion( + Vn100* vn100, + char* firmwareVersionBuffer, + uint32_t firmwareVersionBufferLength); + +/** + * \brief Gets the values in the Serial Baud Rate register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] serialBaudrate The serial baudrate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getSerialBaudRate( + Vn100* vn100, + uint32_t* serialBaudrate); + +/** + * \brief Sets the values of the Serial Baud Rate register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] serialBaudrate Value for the serial baudrate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setSerialBaudRate( + Vn100* vn100, + uint32_t serialBaudrate, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Type register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncDataOutputType The asynchronous data output type value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAsynchronousDataOutputType( + Vn100* vn100, + uint32_t* asyncDataOutputType); + +/** + * \brief Sets the values of the Asynchronous Data Output Type register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] asyncDataOutputType Value for the asynchronous data output type field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setAsynchronousDataOutputType( + Vn100* vn100, + uint32_t asyncDataOutputType, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Frequency register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] asyncDataOutputFrequency The asynchronous data output frequency value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAsynchronousDataOutputFrequency( + Vn100* vn100, + uint32_t* asyncDataOutputFrequency); + +/** + * \brief Sets the values of the Asynchronous Data Output Frequency register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] asyncDataOutputFrequency Value for the asynchronous data output frequency field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setAsynchronousDataOutputFrequency( + Vn100* vn100, + uint32_t asyncDataOutputFrequency, + bool waitForResponse); + +/** + * \brief Gets the values in the Yaw Pitch Roll register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getYawPitchRoll( + Vn100* vn100, + VnYpr* attitude); + +/** + * \brief Gets the values in the Attitude Quaternion register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternion( + Vn100* vn100, + VnQuaternion* attitude); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, Magentic, Accleration, and Angular Rates register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getYawPitchRollMagneticAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Quaternion, Magnetic, Acceleration and Angular + * Rates register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getQuaternionMagneticAccelerationAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic Measurements register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagnetic( + Vn100* vn100, + VnVector3* magnetic); + +/** + * \brief Gets the values in the Acceleration Measurements register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAcceleration( + Vn100* vn100, + VnVector3* acceleration); + +/** + * \brief Gets the values in the Angular Rate Measurements register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAngularRate( + Vn100* vn100, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagneticAccelerationAngularRate( + Vn100* vn100, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] bodyAcceleration The current sensor body acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getYawPitchRollTrueBodyAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* bodyAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] inertialAcceleration The current sensor inertial acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getYawPitchRollTrueInertialAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* inertialAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the VPE Basic Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] enable The enable/disable value of the sensor. + * \param[out] headingMode The heading mode value of the sensor. + * \param[out] filteringMode The filtering mode value of the sensor. + * \param[out] tuningMode The tuning mode value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeControl( + Vn100* vn100, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode); + +/** + * \brief Sets the values of the VPE Basic Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] enable Value for the enable/disable field. + * \param[in] headingMode Value for the heading mode field. + * \param[in] filteringMode Value for the filtering mode field. + * \param[in] tuningMode Value for the tuning mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeControl( + Vn100* vn100, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Magnetometer Basic Tuning register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] baseTuning The current sensor magnetometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor magnetometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor magnetometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeMagnetometerBasicTuning( + Vn100* vn100, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Magnetometer Basic Tuning register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] baseTuning The magnetometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The magnetometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The magnetometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeMagnetometerBasicTuning( + Vn100* vn100, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Accelerometer Basic Tuning register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] baseTuning The current sensor accelerometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor accelerometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor accelerometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getVpeAccelerometerBasicTuning( + Vn100* vn100, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Accelerometer Basic Tuning register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] baseTuning The accelerometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The accelerometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The accelerometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setVpeAccelerometerBasicTuning( + Vn100* vn100, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the IMU Measurements register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magnetic The current sensor uncompensated magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor uncompensated acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \param[out] temperature The temperature value of the sensor. + * \param[out] pressure The pressure value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getImuMeasurements( + Vn100* vn100, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate, + float* temperature, + float* pressure); + +/** + * \brief Gets the values in the Reference Frame Rotation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] c The current sensor C matrix values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getReferenceFrameRotation( + Vn100* vn100, + VnMatrix3x3* c); + +/** + * \brief Sets the values of the Reference Frame Rotation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setReferenceFrameRotation( + Vn100* vn100, + VnMatrix3x3 c, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] syncInMode The input signal synchronization mode value of the sensor. + * \param[out] syncInEdge The input signal synchronization edge selection value of the sensor. + * \param[out] syncInSkipFactor The input signal trigger skip factor value of the sensor. + * \param[out] syncOutMode The output signal synchronization mode value of the sensor. + * \param[out] syncOutPolarity The output signal synchronization polarity value of the sensor. + * \param[out] syncOutSkipFactor The output synchronization signal skip factor value of the sensor. + * \param[out] syncOutPulseWidth The output synchronization signal pulse width value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getSynchronizationControl( + Vn100* vn100, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth); + +/** + * \brief Sets the values of the Synchronization Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] syncInMode Value for the input signal synchronization mode field. + * \param[in] syncInEdge Value for the input signal synchronization edge selection field. + * \param[in] syncInSkipFactor Value for the input signal trigger skip factor field. + * \param[in] syncOutMode Value for the output signal synchronization mode field. + * \param[in] syncOutPolarity Value for the output signal synchronization polarity field. + * \param[in] syncOutSkipFactor Value for the output synchronization signal skip factor field. + * \param[in] syncOutPulseWidth Value for the output synchronization signal pulse width field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setSynchronizationControl( + Vn100* vn100, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Status register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] syncInCount The synchronization in count value of the sensor. + * \param[out] syncInTime The synchronization in time value of the sensor. + * \param[out] syncOutCount The synchronization out count value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getSynchronizationStatus( + Vn100* vn100, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount); + +/** + * \brief Sets the values of the Synchronization Status register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] syncInCount Value for the synchronization in count field. + * \param[in] syncInTime Value for the synchronization in time field. + * \param[in] syncOutCount Value for the synchronization out count field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setSynchronizationStatus( + Vn100* vn100, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount, + bool waitForResponse); + +/** + * \brief Gets the contents of the Delta Theta and Delta Velocity register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] deltaTime Delta time for the integration interval. + * \param[out] deltaTheta Delta rotation vector. + * \param[out] deltaVelocity Delta velocity vector. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getDeltaThetaAndDeltaVelocity( + Vn100* vn100, + float* deltaTime, + VnVector3* deltaTheta, + VnVector3* deltaVelocity); + +/** + * \brief Gets the values in the Acceleration Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getAccelerationCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Acceleration Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setAccelerationCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetic Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagneticCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Magnetic Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setMagneticCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Gyro Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getGyroCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Gyro Compensation register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setGyroCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Retreives the current values of the IMU Filtering Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[out] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[out] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[out] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[out] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[out] magFilterMode Filtering mode for magnetic measurements. + * \param[out] accelFilterMode Filtering mode for acceleration measurements. + * \param[out] gyroFilterMode Filtering mode for gyro measurements. + * \param[out] tempFilterMode Filtering mode for temperature measurements. + * \param[out] presFilterMode Filtering mode for pressure measurements. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getImuFilteringConfiguration( + Vn100* vn100, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode); + +/** + * \brief Sets the values of the IMU Filtering Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[in] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[in] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[in] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[in] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[in] magFilterMode Filtering mode for magnetic measurements. + * \param[in] accelFilterMode Filtering mode for acceleration measurements. + * \param[in] gyroFilterMode Filtering mode for gyro measurements. + * \param[in] tempFilterMode Filtering mode for temperature measurements. + * \param[in] presFilterMode Filtering mode for pressure measurements. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setImuFilteringConfiguration( + Vn100* vn100, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode, + bool waitForResponse); + +/** + * \brief Retreives the current values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] integrationFrame Output frame for delta velocity quantities. + * \param[out] gyroCompensation Compensation to apply to angular rate. + * \param[out] accelCompensation Compensation to apply to accelerations. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getDeltaThetaAndDeltaVelocityConfiguration( + Vn100* vn100, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation); + +/** + * \brief Sets the values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] integrationFrame Output frame for delta velocity quantities. + * \param[in] gyroCompensation Compensation to apply to angular rate. + * \param[in] accelCompensation Compensation to apply to accelerations. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setDeltaThetaAndDeltaVelocityConfiguration( + Vn100* vn100, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetometer Calibration Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] hsiMode The HSIMode value of the sensor. + * \param[out] hsiOutput The HSIOutput value of the sensor. + * \param[out] convergeRate The ConvergeRate value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagnetometerCalibrationControl( + Vn100* vn100, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate); + +/** + * \brief Sets the values of the Magnetometer Calibration Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] hsiMode Value for the HSIMode field. + * \param[in] hsiOutput Value for the HSIOutput field. + * \param[in] convergeRate Value for the ConvergeRate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setMagnetometerCalibrationControl( + Vn100* vn100, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate, + bool waitForResponse); + +/** + * \brief Gets the values in the Calculated Magnetometer Calibration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getCalculatedMagnetometerCalibration( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Gets the values in the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] magneticReference The current sensor magnetic reference vector (X,Y,Z) values. + * \param[out] gravityReference The current sensor gravity reference vector (X,Y,Z) values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getMagneticGravityReferenceVectors( + Vn100* vn100, + VnVector3* magneticReference, + VnVector3* gravityReference); + +/** + * \brief Sets the values of the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] magneticReference The magnetic reference vector (X,Y,Z) values to write to the sensor. + * \param[in] gravityReference The gravity reference vector (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setMagneticGravityReferenceVectors( + Vn100* vn100, + VnVector3 magneticReference, + VnVector3 gravityReference, + bool waitForResponse); + +/** + * \brief Gets the values in the Communication Protocol Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] serialCount The serial count value of the sensor. + * \param[out] serialStatus The serial status value of the sensor. + * \param[out] spiCount The SPI count value of the sensor. + * \param[out] spiStatus The SPI status value of the sensor. + * \param[out] serialChecksum The serial checksum value of the sensor. + * \param[out] spiChecksum The SPI checksum value of the sensor. + * \param[out] errorMode The error mode value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getCommunicationProtocolControl( + Vn100* vn100, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode); + +/** + * \brief Sets the values of the Communication Protocol Control register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] serialCount Value for the serial count field. + * \param[in] serialStatus Value for the serial status field. + * \param[in] spiCount Value for the SPI count field. + * \param[in] spiStatus Value for the SPI status field. + * \param[in] serialChecksum Value for the serial checksum field. + * \param[in] spiChecksum Value for the SPI checksum field. + * \param[in] errorMode Value for the error mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setCommunicationProtocolControl( + Vn100* vn100, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode, + bool waitForResponse); + +/** + * \brief Gets the values in the Reference Vector Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[out] useMagModel The UseMagModel field. + * \param[out] useGravityModel The UseGravityModel field. + * \param[out] recalcThreshold The RecalcThreshold field. + * \param[out] year The Year field. + * \param[out] lla The Lattitude, Longitude, Altitude fields. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_getReferenceVectorConfiguration( + Vn100* vn100, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint32_t* recalcThreshold, + float* year, + VnVector3* lla); + +/** + * \brief Sets the values in the Reference Vector Configuration register. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] useMagModel The UseMagModel field. + * \param[in] useGravityModel The UseGravityModel field. + * \param[in] recalcThreshold The RecalcThreshold field. + * \param[in] year The Year field. + * \param[in] lla The Lattitude, Longitude, Altitude fields. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_setReferenceVectorConfiguration( + Vn100* vn100, + uint8_t useMagModel, + uint8_t useGravityModel, + uint32_t recalcThreshold, + float year, + VnVector3 lla, + bool waitForResponse); + +/** + * \brief Pauses the asynchronous data output. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_pauseAsyncOutputs( + Vn100* vn100, + bool waitForResponse); + +/** + * \brief Resumes the asynchronous data output. + * + * \param[in] vn100 Pointer to the Vn100 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn100_resumeAsyncOutputs( + Vn100* vn100, + bool waitForResponse); + +#ifdef __cplusplus +} +#endif + +#endif /* _VN100_H_ */ diff --git a/src/vectornav/include/vectornav/vn200.h b/src/vectornav/include/vectornav/vn200.h new file mode 100755 index 0000000..97dae85 --- /dev/null +++ b/src/vectornav/include/vectornav/vn200.h @@ -0,0 +1,1746 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to devices based on VectorNav's VN-200 + * family of orientation sensors. + */ +#ifndef _VN200_H_ +#define _VN200_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1500 + /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ + #include "vnint.h" +#else + #include +#endif + +#include "vndevice.h" +#include "vncp_services.h" +#include "vn_kinematics.h" +#include "vn_linearAlgebra.h" + +#if defined(EXPORT_TO_DLL) + #define DLL_EXPORT __declspec(dllexport) +#else + /** Don't compile the library with support for DLL function export. */ + #define DLL_EXPORT +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \brief Holds connection information for accessing a VN-200 device. + */ +typedef struct { + char* portName; /**< The name of the serial port. */ + int baudRate; /**< The baudrate of the serial port. */ + bool isConnected; /**< Inidicates if the serial port is open. */ + VnDevice vndevice; /**< Pointer to internally used data. */ +} Vn200; + +/** + * \brief Connects to a VectorNav VN-200 device. + * + * \param[out] newVn200 An uninitialized Vn200 control object should be passed in. + * \param[in] portName The name of the COM port to connect to. + * \param[in] baudrate The baudrate to connect at. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_connect( + Vn200* newVn200, + const char* portName, + int baudrate); + +/** + * \brief Disconnects from the VN-200 device and disposes of any internal resources. + * + * \param vn200 Pointer to the Vn200 control object. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_disconnect( + Vn200* vn200); + +/** + * \brief Allows registering a function which will be called whenever a new + * asynchronous data packet is received from the VN-200 module. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] listener The function pointer to be called when asynchronous data + * is received. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_registerAsyncDataReceivedListener( + Vn200* vn200, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when new asynchronous data is received. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_unregisterAsyncDataReceivedListener( + Vn200* vn200, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Allows registering a function which will be called whenever an error + * code is received from the VN-200 module. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] listener The function pointer to be called when an error code + * is received. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vn200_registerErrorCodeReceivedListener( + Vn200* vn200, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when error codes from the VN-200 module are received. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vn200_unregisterErrorCodeReceivedListener( + Vn200* vn200, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Checks if we are able to send and receive communication with the VN-200 sensor. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * + * \return VN_TRUE if the library was able to send and receive a valid response from the VN-200 sensor; otherwise VN_FALSE. + */ +DLL_EXPORT bool vn200_verifyConnectivity( + Vn200* vn200); + +/** + * \brief Commands the VN-200 to copy the current filter bias estimates into + * register 74. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setFilterBias( + Vn200* vn200, + bool waitForResponse); + +/** + * \brief Gets the current configuration of the requested binary output register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to query for its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup2Selections The configured output data types for output group 2. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup4Selections The configured output data types for output group 4. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * \param[out] outputGroup6Selections The configured output data types for output group 6. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getBinaryOutputConfiguration( + Vn200* vn200, + uint8_t binaryOutputRegisterId, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections); + +/** + * \brief Gets the current configuration of the binary output register 1. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup2Selections The configured output data types for output group 2. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup4Selections The configured output data types for output group 4. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * \param[out] outputGroup6Selections The configured output data types for output group 6. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getBinaryOutput1Configuration( + Vn200* vn200, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections); + +/** + * \brief Gets the current configuration of the binary output register 2. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup2Selections The configured output data types for output group 2. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup4Selections The configured output data types for output group 4. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * \param[out] outputGroup6Selections The configured output data types for output group 6. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getBinaryOutput2Configuration( + Vn200* vn200, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections); + +/** + * \brief Gets the current configuration of the binary output register 3. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup2Selections The configured output data types for output group 2. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup4Selections The configured output data types for output group 4. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * \param[out] outputGroup6Selections The configured output data types for output group 6. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getBinaryOutput3Configuration( + Vn200* vn200, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections); + +/** + * \brief Sets the configuration of the requested binary output register. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to set its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup2Selections The output data types for output group 2. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup4Selections The output data types for output group 4. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[out] outputGroup6Selections The output data types for output group 6. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setBinaryOutputConfiguration( + Vn200* vn200, + uint8_t binaryOutputRegisterId, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 1. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup2Selections The output data types for output group 2. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup4Selections The output data types for output group 4. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[out] outputGroup6Selections The output data types for output group 6. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setBinaryOutput1Configuration( + Vn200* vn200, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 2. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup2Selections The output data types for output group 2. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup4Selections The output data types for output group 4. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[out] outputGroup6Selections The output data types for output group 6. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setBinaryOutput2Configuration( + Vn200* vn200, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse); + +/** + * \brief Sets the current configuration of the binary output register 3. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup2Selections The output data types for output group 2. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup4Selections The output data types for output group 4. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[out] outputGroup6Selections The output data types for output group 6. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setBinaryOutput3Configuration( + Vn200* vn200, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse); + +/** + * \brief Gets the values in the GPS Configuration register. + * + * \note This function is only suitable for VN-200 devices with firmware versions + * earlier than v1.0. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] mode The mode value of the sensor. + * \param[out] nmeaSerial1 The NMEA_Serial1 value of the sensor. + * \param[out] nmeaSerial2 The NMEA_Serial2 value of the sensor. + * \param[out] nmeaRate The NMEA_Rate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGpsConfiguration_preFirmwareVersion1d0( + Vn200* vn200, + unsigned char* mode, + unsigned char* nmeaSerial1, + unsigned char* nmeaSerial2, + unsigned char* nmeaRate); + +/** + * \brief Sets the values of the GPS Configuration register. + * + * \note This function is only suitable for VN-200 devices with firmware versions + * earlier than v1.0. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] mode Value for the mode field. + * \param[in] nmeaSerial1 Value for the NMEA_Serial1 field. + * \param[in] nmeaSerial2 Value for the NMEA_Serial2 field. + * \param[in] nmeaRate Value for the NMEA_Rate field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setGpsConfiguration_preFirmwareVersion1d0( + Vn200* vn200, + unsigned char mode, + unsigned char nmeaSerial1, + unsigned char nmeaSerial2, + unsigned char nmeaRate, + bool waitForResponse); + +/** + * \brief Gets the values in the GPS Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] mode The mode value of the sensor. + * \param[out] ppsSource GPS PPS mode. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGpsConfiguration( + Vn200* vn200, + unsigned char* mode, + unsigned char* ppsSource); + +/** + * \brief Sets the values of the GPS Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] mode Value for the mode field. + * \param[in] ppsSource Value for the PpsSource field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setGpsConfiguration( + Vn200* vn200, + unsigned char mode, + unsigned char ppsSource, + bool waitForResponse); + + +/** + * \brief Gets the values in the GPS Antenna Offset register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] position The current sensor relative postion of GPS antenna (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGpsAntennaOffset( + Vn200* vn200, + VnVector3* position); + +/** + * \brief Sets the values of the GPS Antenna Offset register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] position The relative postion of GPS antenna (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setGpsAntennaOffset( + Vn200* vn200, + VnVector3 position, + bool waitForResponse); + +/** + * \brief Gets the values in the GPS Solution - LLA register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] gpsTime The GPS time of week in seconds value of the sensor. + * \param[out] gpsWeek The GPS week value of the sensor. + * \param[out] gpsFix The GPS fix type value of the sensor. + * \param[out] numberOfSatellites The number of GPS satellites used in solution value of the sensor. + * \param[out] lattitudeLongitudeAltitude The current sensor latitude, longitude, and altitude values. + * \param[out] nedVelocity The current sensor velocity measurements (X,Y,Z) in north, east, down directions values. + * \param[out] positionAccuracy The current sensor position accuracy (X,Y,Z) values. + * \param[out] speedAccuracy The speed accuracy estimate value of the sensor. + * \param[out] timeAccuracy The time accuracy estimate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGpsSolutionLla( + Vn200* vn200, + double* gpsTime, + unsigned short* gpsWeek, + unsigned char* gpsFix, + unsigned char* numberOfSatellites, + VnVector3* lattitudeLongitudeAltitude, + VnVector3* nedVelocity, + VnVector3* positionAccuracy, + float* speedAccuracy, + float* timeAccuracy); + +/** + * \brief Gets the values in the GPS Solution - ECEF register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] gpsTime The GPS time of week in seconds value of the sensor. + * \param[out] gpsWeek The GPS week value of the sensor. + * \param[out] gpsFix The GPS fix type value of the sensor. + * \param[out] numberOfSatellites The number of GPS satellites used in solution value of the sensor. + * \param[out] position The current sensor's ECEF values. + * \param[out] velocity The current sensor velocity measurements (X,Y,Z) in ECEF. + * \param[out] positionAccuracy The current sensor position accuracy (X,Y,Z) in ECEF. + * \param[out] speedAccuracy The speed accuracy estimate value of the sensor. + * \param[out] timeAccuracy The time accuracy estimate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGpsSolutionEcef( + Vn200* vn200, + double* gpsTime, + unsigned short* gpsWeek, + unsigned char* gpsFix, + unsigned char* numberOfSatellites, + VnVector3* position, + VnVector3* velocity, + VnVector3* positionAccuracy, + float* speedAccuracy, + float* timeAccuracy); + +/** + * \brief Gets the values in the INS Solution - LLA register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] gpsTime The GPS time of week in seconds value of the sensor. + * \param[out] gpsWeek The GPS week value of the sensor. + * \param[out] status The status flags for the INS filter value of the sensor. + * \param[out] ypr The current sensor heading, pitch, and roll values. + * \param[out] lattitudeLongitudeAltitude The current sensor latitude, longitude, and altitude values. + * \param[out] nedVelocity The current sensor velocity measurements (X,Y,Z) in north, east, down directions values. + * \param[out] attitudeUncertainty The attitude uncertainty value of the sensor. + * \param[out] positionUncertainty The position uncertainty value of the sensor. + * \param[out] velocityUncertainty The velocity uncertainty value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getInsSolutionLla( + Vn200* vn200, + double* gpsTime, + unsigned short* gpsWeek, + unsigned short* status, + VnVector3* ypr, + VnVector3* lattitudeLongitudeAltitude, + VnVector3* nedVelocity, + float* attitudeUncertainty, + float* positionUncertainty, + float* velocityUncertainty); + +/** + * \brief Gets the values in the INS Solution - ECEF register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] gpsTime The GPS time of week in seconds value of the sensor. + * \param[out] gpsWeek The GPS week value of the sensor. + * \param[out] status The status flags for the INS filter value of the sensor. + * \param[out] ypr The current sensor heading, pitch, and roll values. + * \param[out] position The current sensor position in the ECEF frame. + * \param[out] velocity The current sensor velocity. + * \param[out] attitudeUncertainty The attitude uncertainty value of the sensor. + * \param[out] positionUncertainty The position uncertainty value of the sensor. + * \param[out] velocityUncertainty The velocity uncertainty value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getInsSolutionEcef( + Vn200* vn200, + double* gpsTime, + uint16_t* gpsWeek, + uint16_t* status, + VnVector3* ypr, + VnVector3* position, + VnVector3* velocity, + float* attitudeUncertainty, + float* positionUncertainty, + float* velocityUncertainty); + +/** + * \brief Gets the values in the INS State - LLA register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] ypr The current sensor heading, pitch, and roll values. + * \param[out] lla The estimated position in latitude, longitude, and altitude. + * \param[out] velocity The current sensor velocity. + * \param[out] accel The estimated acceleration in body frame. + * \param[out] angularRate The estimate angular rate in body frame. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getInsStateLla( + Vn200* vn200, + VnVector3* ypr, + VnVector3* lla, + VnVector3* velocity, + VnVector3* accel, + VnVector3* angularRate); + +/** + * \brief Gets the values in the INS State - ECEF register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] ypr The current sensor heading, pitch, and roll values. + * \param[out] position The estimated position in ECEF. + * \param[out] velocity The current sensor velocity. + * \param[out] accel The estimated acceleration in body frame. + * \param[out] angularRate The estimate angular rate in body frame. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getInsStateEcef( + Vn200* vn200, + VnVector3* ypr, + VnVector3* position, + VnVector3* velocity, + VnVector3* accel, + VnVector3* angularRate); + +/** + * \brief Gets the values in the INS Basic Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] scenario INS mode. + * \param[out] ahrsAiding Enables AHRS attitude aiding. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getInsBasicConfiguration( + Vn200* vn200, + uint8_t* scenario, + uint8_t* ahrsAiding); + +/** + * \brief Sets the values of the INS Basic Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] scenario INS mode. + * \param[in] ahrsAiding Enables AHRS attitude aiding. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setInsBasicConfiguration( + Vn200* vn200, + uint8_t scenario, + uint8_t ahrsAiding, + bool waitForResponse); + +/** + * \brief Gets the values in the Startup Filter Bias Estimate register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] gyroBias Gyro bias field. + * \param[out] accelBias Accelerometer bias field. + * \param[out] pressureBias Pressure bias field. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getStartupFilterBiasEstimate( + Vn200* vn200, + VnVector3* gyroBias, + VnVector3* accelBias, + float* pressureBias); + +/** + * \brief Sets the values of the Startup Filter Bias Estimate register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] gyroBias Gyro bias field. + * \param[in] accelBias Accelerometer bias field. + * \param[in] pressureBias Pressure bias field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setStartupFilterBiasEstimate( + Vn200* vn200, + VnVector3 gyroBias, + VnVector3 accelBias, + float pressureBias, + bool waitForResponse); + +/** + * \brief Retrieves the associated timeout value for the Vn200 object. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * + * \return The timeout value in milliseconds. -1 indicates that timeouts are + * not used. + */ +DLL_EXPORT int vn200_get_timeout( + Vn200* vn200); + +/** + * \brief Sets the timeout value for the reading values from the VectorNav sensor. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] timeout The timeout value in milliseconds. Specify -1 to not use + * any timeouts. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_set_timeout( + Vn200* vn200, + int timeout); + +/** + * \brief Retrieves the most recent stored asynchronous data. + * + * \param[in] vn200 Pointer to the Vn200 object. + * \param[out] curData Returned pointer current asychronous data. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getCurrentAsyncData( + Vn200* vn200, + VnDeviceCompositeData* curData); + +/** + * \brief Commands the VectorNav unit to write its current register setting to + * non-volatile memory. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_writeSettings( + Vn200* vn200, + bool waitForResponse); + +/** + * \brief Commands the VectorNav unit to revert its settings to factory defaults. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_restoreFactorySettings( + Vn200* vn200, + bool waitForResponse); + +/** + * \brief Commands the VectorNav module to reset itself. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_reset( + Vn200* vn200); + +/** + * \brief Gets the values in the User Tag register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] userTagBuffer Buffer to store the response. Must have a length of at least 21 characters. + * \param[in] userTagBufferLength Length of the provided userTagBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getUserTag( + Vn200* vn200, + char* userTagBuffer, + uint32_t userTagBufferLength); + +/** + * \brief Sets the values of the User Tag register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] userTagData Array containg the data to send. Length must be equal to or less than 20 characters. + * \param[in] userTagDataLength Length of the data to send in the userTagData array. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setUserTag( + Vn200* vn200, + char* userTagData, + uint32_t userTagDataLength, + bool waitForResponse); + +/** + * \brief Gets the values in the Model Number register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] modelBuffer Buffer to store the response. Must have a length of at least 25 characters. + * \param[in] modelBufferLength Length of the provided modelBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getModelNumber( + Vn200* vn200, + char* modelBuffer, + uint32_t modelBufferLength); + +/** + * \brief Gets the values in the Hardware Revision register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] hardwareRevision The hardware revision value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getHardwareRevision( + Vn200* vn200, + int32_t* hardwareRevision); + +/** + * \brief Gets the values in the Serial Number register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] serialNumberBuffer Buffer to store the response. Must have a length of at least 13 characters. + * \param[in] serialNumberBufferLength Length of the provided serialNumberBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getSerialNumber( + Vn200* vn200, + char* serialNumberBuffer, + uint32_t serialNumberBufferLength); + +/** + * \brief Gets the value in the Firmware Version register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] firmwareVersionBuffer Buffer to store the response. Must have a length of at least 16 characters. + * \param[in] firmwareVersionBufferLength Length of the provided firmwareVersionBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getFirmwareVersion( + Vn200* vn200, + char* firmwareVersionBuffer, + uint32_t firmwareVersionBufferLength); + +/** + * \brief Gets the values in the Serial Baud Rate register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] serialBaudrate The serial baudrate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getSerialBaudRate( + Vn200* vn200, + uint32_t* serialBaudrate); + +/** + * \brief Sets the values of the Serial Baud Rate register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] serialBaudrate Value for the serial baudrate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setSerialBaudRate( + Vn200* vn200, + uint32_t serialBaudrate, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Type register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncDataOutputType The asynchronous data output type value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getAsynchronousDataOutputType( + Vn200* vn200, + uint32_t* asyncDataOutputType); + +/** + * \brief Sets the values of the Asynchronous Data Output Type register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] asyncDataOutputType Value for the asynchronous data output type field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setAsynchronousDataOutputType( + Vn200* vn200, + uint32_t asyncDataOutputType, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Frequency register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] asyncDataOutputFrequency The asynchronous data output frequency value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getAsynchronousDataOutputFrequency( + Vn200* vn200, + uint32_t* asyncDataOutputFrequency); + +/** + * \brief Sets the values of the Asynchronous Data Output Frequency register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] asyncDataOutputFrequency Value for the asynchronous data output frequency field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setAsynchronousDataOutputFrequency( + Vn200* vn200, + uint32_t asyncDataOutputFrequency, + bool waitForResponse); + +/** + * \brief Gets the values in the Yaw Pitch Roll register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getYawPitchRoll( + Vn200* vn200, + VnYpr* attitude); + +/** + * \brief Gets the values in the Attitude Quaternion register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor Quaterion values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getQuaternion( + Vn200* vn200, + VnQuaternion* attitude); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, Magentic, Accleration, and Angular Rates register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getYawPitchRollMagneticAccelerationAngularRate( + Vn200* vn200, + VnYpr* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Quaternion, Magnetic, Acceleration and Angular + * Rates register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getQuaternionMagneticAccelerationAngularRate( + Vn200* vn200, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic Measurements register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getMagnetic( + Vn200* vn200, + VnVector3* magnetic); + +/** + * \brief Gets the values in the Acceleration Measurements register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getAcceleration( + Vn200* vn200, + VnVector3* acceleration); + +/** + * \brief Gets the values in the Angular Rate Measurements register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getAngularRate( + Vn200* vn200, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getMagneticAccelerationAngularRate( + Vn200* vn200, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] bodyAcceleration The current sensor body acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getYawPitchRollTrueBodyAccelerationAngularRate( + Vn200* vn200, + VnYpr* attitude, + VnVector3* bodyAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] inertialAcceleration The current sensor inertial acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getYawPitchRollTrueInertialAccelerationAngularRate( + Vn200* vn200, + VnYpr* attitude, + VnVector3* inertialAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the VPE Basic Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] enable The enable/disable value of the sensor. + * \param[out] headingMode The heading mode value of the sensor. + * \param[out] filteringMode The filtering mode value of the sensor. + * \param[out] tuningMode The tuning mode value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getVpeControl( + Vn200* vn200, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode); + +/** + * \brief Sets the values of the VPE Basic Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] enable Value for the enable/disable field. + * \param[in] headingMode Value for the heading mode field. + * \param[in] filteringMode Value for the filtering mode field. + * \param[in] tuningMode Value for the tuning mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setVpeControl( + Vn200* vn200, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Magnetometer Basic Tuning register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] baseTuning The current sensor magnetometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor magnetometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor magnetometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getVpeMagnetometerBasicTuning( + Vn200* vn200, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Magnetometer Basic Tuning register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] baseTuning The magnetometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The magnetometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The magnetometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setVpeMagnetometerBasicTuning( + Vn200* vn200, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Accelerometer Basic Tuning register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] baseTuning The current sensor accelerometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor accelerometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor accelerometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getVpeAccelerometerBasicTuning( + Vn200* vn200, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Accelerometer Basic Tuning register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] baseTuning The accelerometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The accelerometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The accelerometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setVpeAccelerometerBasicTuning( + Vn200* vn200, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the IMU Measurements register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] magnetic The current sensor uncompensated magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor uncompensated acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \param[out] temperature The temperature value of the sensor. + * \param[out] pressure The pressure value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getImuMeasurements( + Vn200* vn200, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate, + float* temperature, + float* pressure); + +/** + * \brief Gets the values in the Reference Frame Rotation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] c The current sensor C matrix values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getReferenceFrameRotation( + Vn200* vn200, + VnMatrix3x3* c); + +/** + * \brief Sets the values of the Reference Frame Rotation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setReferenceFrameRotation( + Vn200* vn200, + VnMatrix3x3 c, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] syncInMode The input signal synchronization mode value of the sensor. + * \param[out] syncInEdge The input signal synchronization edge selection value of the sensor. + * \param[out] syncInSkipFactor The input signal trigger skip factor value of the sensor. + * \param[out] syncOutMode The output signal synchronization mode value of the sensor. + * \param[out] syncOutPolarity The output signal synchronization polarity value of the sensor. + * \param[out] syncOutSkipFactor The output synchronization signal skip factor value of the sensor. + * \param[out] syncOutPulseWidth The output synchronization signal pulse width value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getSynchronizationControl( + Vn200* vn200, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth); + +/** + * \brief Sets the values of the Synchronization Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] syncInMode Value for the input signal synchronization mode field. + * \param[in] syncInEdge Value for the input signal synchronization edge selection field. + * \param[in] syncInSkipFactor Value for the input signal trigger skip factor field. + * \param[in] syncOutMode Value for the output signal synchronization mode field. + * \param[in] syncOutPolarity Value for the output signal synchronization polarity field. + * \param[in] syncOutSkipFactor Value for the output synchronization signal skip factor field. + * \param[in] syncOutPulseWidth Value for the output synchronization signal pulse width field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setSynchronizationControl( + Vn200* vn200, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Status register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] syncInCount The synchronization in count value of the sensor. + * \param[out] syncInTime The synchronization in time value of the sensor. + * \param[out] syncOutCount The synchronization out count value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getSynchronizationStatus( + Vn200* vn200, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount); + +/** + * \brief Sets the values of the Synchronization Status register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] syncInCount Value for the synchronization in count field. + * \param[in] syncInTime Value for the synchronization in time field. + * \param[in] syncOutCount Value for the synchronization out count field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setSynchronizationStatus( + Vn200* vn200, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount, + bool waitForResponse); + +/** + * \brief Gets the contents of the Delta Theta and Delta Velocity register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] deltaTime Delta time for the integration interval. + * \param[out] deltaTheta Delta rotation vector. + * \param[out] deltaVelocity Delta velocity vector. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getDeltaThetaAndDeltaVelocity( + Vn200* vn200, + float* deltaTime, + VnVector3* deltaTheta, + VnVector3* deltaVelocity); + +/** + * \brief Gets the values in the Acceleration Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getAccelerationCompensation( + Vn200* vn200, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Acceleration Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setAccelerationCompensation( + Vn200* vn200, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetic Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getMagneticCompensation( + Vn200* vn200, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Magnetic Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setMagneticCompensation( + Vn200* vn200, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Gyro Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getGyroCompensation( + Vn200* vn200, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Gyro Compensation register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setGyroCompensation( + Vn200* vn200, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Retreives the current values of the IMU Filtering Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[out] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[out] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[out] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[out] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[out] magFilterMode Filtering mode for magnetic measurements. + * \param[out] accelFilterMode Filtering mode for acceleration measurements. + * \param[out] gyroFilterMode Filtering mode for gyro measurements. + * \param[out] tempFilterMode Filtering mode for temperature measurements. + * \param[out] presFilterMode Filtering mode for pressure measurements. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getImuFilteringConfiguration( + Vn200* vn200, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode); + +/** + * \brief Sets the values of the IMU Filtering Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[in] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[in] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[in] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[in] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[in] magFilterMode Filtering mode for magnetic measurements. + * \param[in] accelFilterMode Filtering mode for acceleration measurements. + * \param[in] gyroFilterMode Filtering mode for gyro measurements. + * \param[in] tempFilterMode Filtering mode for temperature measurements. + * \param[in] presFilterMode Filtering mode for pressure measurements. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setImuFilteringConfiguration( + Vn200* vn200, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode, + bool waitForResponse); + +/** + * \brief Retreives the current values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] integrationFrame Output frame for delta velocity quantities. + * \param[out] gyroCompensation Compensation to apply to angular rate. + * \param[out] accelCompensation Compensation to apply to accelerations. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getDeltaThetaAndDeltaVelocityConfiguration( + Vn200* vn200, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation); + +/** + * \brief Sets the values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] integrationFrame Output frame for delta velocity quantities. + * \param[in] gyroCompensation Compensation to apply to angular rate. + * \param[in] accelCompensation Compensation to apply to accelerations. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setDeltaThetaAndDeltaVelocityConfiguration( + Vn200* vn200, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetometer Calibration Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] hsiMode The HSIMode value of the sensor. + * \param[out] hsiOutput The HSIOutput value of the sensor. + * \param[out] convergeRate The ConvergeRate value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getMagnetometerCalibrationControl( + Vn200* vn200, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate); + +/** + * \brief Sets the values of the Magnetometer Calibration Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] hsiMode Value for the HSIMode field. + * \param[in] hsiOutput Value for the HSIOutput field. + * \param[in] convergeRate Value for the ConvergeRate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setMagnetometerCalibrationControl( + Vn200* vn200, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate, + bool waitForResponse); + +/** + * \brief Gets the values in the Calculated Magnetometer Calibration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getCalculatedMagnetometerCalibration( + Vn200* vn200, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Gets the values in the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] magneticReference The current sensor magnetic reference vector (X,Y,Z) values. + * \param[out] gravityReference The current sensor gravity reference vector (X,Y,Z) values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getMagneticGravityReferenceVectors( + Vn200* vn200, + VnVector3* magneticReference, + VnVector3* gravityReference); + +/** + * \brief Sets the values of the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] magneticReference The magnetic reference vector (X,Y,Z) values to write to the sensor. + * \param[in] gravityReference The gravity reference vector (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setMagneticGravityReferenceVectors( + Vn200* vn200, + VnVector3 magneticReference, + VnVector3 gravityReference, + bool waitForResponse); + +/** + * \brief Gets the values in the Communication Protocol Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] serialCount The serial count value of the sensor. + * \param[out] serialStatus The serial status value of the sensor. + * \param[out] spiCount The SPI count value of the sensor. + * \param[out] spiStatus The SPI status value of the sensor. + * \param[out] serialChecksum The serial checksum value of the sensor. + * \param[out] spiChecksum The SPI checksum value of the sensor. + * \param[out] errorMode The error mode value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getCommunicationProtocolControl( + Vn200* vn200, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode); + +/** + * \brief Sets the values of the Communication Protocol Control register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] serialCount Value for the serial count field. + * \param[in] serialStatus Value for the serial status field. + * \param[in] spiCount Value for the SPI count field. + * \param[in] spiStatus Value for the SPI status field. + * \param[in] serialChecksum Value for the serial checksum field. + * \param[in] spiChecksum Value for the SPI checksum field. + * \param[in] errorMode Value for the error mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setCommunicationProtocolControl( + Vn200* vn200, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode, + bool waitForResponse); + +/** + * \brief Gets the values in the Reference Vector Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[out] useMagModel The UseMagModel field. + * \param[out] useGravityModel The UseGravityModel field. + * \param[out] recalcThreshold The RecalcThreshold field. + * \param[out] year The Year field. + * \param[out] lla The Lattitude, Longitude, Altitude fields. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_getReferenceVectorConfiguration( + Vn200* vn200, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint32_t* recalcThreshold, + float* year, + VnVector3* lla); + +/** + * \brief Sets the values in the Reference Vector Configuration register. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] useMagModel The UseMagModel field. + * \param[in] useGravityModel The UseGravityModel field. + * \param[in] recalcThreshold The RecalcThreshold field. + * \param[in] year The Year field. + * \param[in] lla The Lattitude, Longitude, Altitude fields. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_setReferenceVectorConfiguration( + Vn200* vn200, + uint8_t useMagModel, + uint8_t useGravityModel, + uint32_t recalcThreshold, + float year, + VnVector3 lla, + bool waitForResponse); + +/** + * \brief Pauses the asynchronous data output. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_pauseAsyncOutputs( + Vn200* vn200, + bool waitForResponse); + +/** + * \brief Resumes the asynchronous data output. + * + * \param[in] vn200 Pointer to the Vn200 control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vn200_resumeAsyncOutputs( + Vn200* vn200, + bool waitForResponse); + +#ifdef __cplusplus +} +#endif + +#endif /* _VN200_H_ */ diff --git a/src/vectornav/include/vectornav/vn_common.h b/src/vectornav/include/vectornav/vn_common.h new file mode 100755 index 0000000..8cc8dc2 --- /dev/null +++ b/src/vectornav/include/vectornav/vn_common.h @@ -0,0 +1,44 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to features commonly used throughout the + * entire VectorNav C/C++ Library. + */ +#ifndef _VN_COMMON_H_ +#define _VN_COMMON_H_ + +#include "vn_errorCodes.h" + +/** + * \brief Retrieves the last error code encountered. Note that the stored error + * code is cleared on the first call to this function. + * + * \return The VectorNav error code. + */ +VN_ERROR_CODE vn_getErrorCode(); + +#endif /* _VN_COMMON_H_ */ \ No newline at end of file diff --git a/src/vectornav/include/vectornav/vn_errorCodes.h b/src/vectornav/include/vectornav/vn_errorCodes.h new file mode 100755 index 0000000..7d71394 --- /dev/null +++ b/src/vectornav/include/vectornav/vn_errorCodes.h @@ -0,0 +1,71 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file defines the error codes used within the VectorNav C/C++ + * Library. + */ +#ifndef _VN_ERRORCODES_H_ +#define _VN_ERRORCODES_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1500 + /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ + #include "vnint.h" +#else + #include +#endif + +/** Type define for VectorNav error codes. */ +typedef uint32_t VN_ERROR_CODE; + +/** + * @defgroup VN_ERROR_CODE VectorNav Error Code Definitions + * + * @{ + */ +#define VNERR_NO_ERROR 0 /**< No error occurred. */ +#define VNERR_UNKNOWN_ERROR 1 /**< An unknown error occurred. */ +#define VNERR_NOT_IMPLEMENTED 2 /**< The operation is not implemented. */ +#define VNERR_TIMEOUT 3 /**< Operation timed out. */ +#define VNERR_INVALID_VALUE 4 /**< Invalid value was provided. */ +#define VNERR_FILE_NOT_FOUND 5 /**< The file was not found. */ +#define VNERR_NOT_CONNECTED 6 /**< Not connected to the sensor. */ +#define VNERR_PERMISSION_DENIED 7 /**< Permission is denied. */ +#define VNERR_SENSOR_HARD_FAULT 8 /**< Sensor experienced a hard fault error. */ +#define VNERR_SENSOR_SERIAL_BUFFER_OVERFLOW 9 /**< Sensor experienced a serial buffer overflow error. */ +#define VNERR_SENSOR_INVALID_CHECKSUM 10 /**< Sensor reported an invalid checksum error. */ +#define VNERR_SENSOR_INVALID_COMMAND 11 /**< Sensor reported an invalid command error. */ +#define VNERR_SENSOR_NOT_ENOUGH_PARAMETERS 12 /**< Sensor reported a not enough parameters error. */ +#define VNERR_SENSOR_TOO_MANY_PARAMETERS 13 /**< Sensor reported a too many parameters error. */ +#define VNERR_SENSOR_INVALID_PARAMETER 14 /**< Sensor reported an invalid parameter error. */ +#define VNERR_SENSOR_UNAUTHORIZED_ACCESS 15 /**< Sensor reported an unauthorized access error. */ +#define VNERR_SENSOR_WATCHDOG_RESET 16 /**< Sensor reported a watchdog reset error. */ +#define VNERR_SENSOR_OUTPUT_BUFFER_OVERFLOW 17 /**< Sensor reported an output buffer overflow error. */ +#define VNERR_SENSOR_INSUFFICIENT_BAUD_RATE 18 /**< Sensor reported an insufficient baudrate error. */ +#define VNERR_SENSOR_ERROR_BUFFER_OVERFLOW 19 /**< Sensor reported an error buffer overflow error. */ +/** @} */ + +#endif /* _VN_ERRORCODES_H_ */ diff --git a/src/vectornav/include/vectornav/vn_kinematics.h b/src/vectornav/include/vectornav/vn_kinematics.h new file mode 100755 index 0000000..4e5dcc1 --- /dev/null +++ b/src/vectornav/include/vectornav/vn_kinematics.h @@ -0,0 +1,52 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to the VectorNav Kinematics library. + */ +#ifndef _VN_KINEMATICS_H +#define _VN_KINEMATICS_H + +/** + * \brief Holds attitude data expressed in yaw, pitch, roll format. + */ +typedef struct { + double yaw; /**< Yaw */ + double pitch; /**< Pitch */ + double roll; /**< Roll */ +} VnYpr; + +/** + * \brief Holds attitude data expressed in quaternion format. + */ +typedef struct { + double x; /**< X */ + double y; /**< Y */ + double z; /**< Z */ + double w; /**< W */ +} VnQuaternion; + +#endif /* _VN_KINEMATICS_H */ diff --git a/src/vectornav/include/vectornav/vn_linearAlgebra.h b/src/vectornav/include/vectornav/vn_linearAlgebra.h new file mode 100755 index 0000000..6698ad4 --- /dev/null +++ b/src/vectornav/include/vectornav/vn_linearAlgebra.h @@ -0,0 +1,62 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to the linear algebra features. + * + * Notes + * - Indexes used within the math library use 0 based indexing. For example, + * the first component of a 3 dimensional vector is referenced in code + * as vector3->c0. + */ +#ifndef _VN_LINEAR_ALGEBRA_H_ +#define _VN_LINEAR_ALGEBRA_H_ + +/** + * \brief A vector of length 3. + */ +typedef struct { + double c0; /**< Component 0 */ + double c1; /**< Component 1 */ + double c2; /**< Component 2 */ +} VnVector3; + +/** + * \brief A 3x3 matrix. + */ +typedef struct { + double c00; /**< Component 0,0 */ + double c01; /**< Component 0,1 */ + double c02; /**< Component 0,2 */ + double c10; /**< Component 1,0 */ + double c11; /**< Component 1,1 */ + double c12; /**< Component 1,2 */ + double c20; /**< Component 2,0 */ + double c21; /**< Component 2,1 */ + double c22; /**< Component 2,2 */ +} VnMatrix3x3; + +#endif /* _VN_LINEAR_ALGEBRA_H_ */ \ No newline at end of file diff --git a/src/vectornav/include/vectornav/vn_math.h b/src/vectornav/include/vectornav/vn_math.h new file mode 100755 index 0000000..3ae04a4 --- /dev/null +++ b/src/vectornav/include/vectornav/vn_math.h @@ -0,0 +1,42 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access the math module of the VectorNav C/C++ + * Library. + * + * Notes + * - Indexes used within the math library use 0 based indexing. For example, + * the first component of a 3 dimensional vector is referenced in code + * as vector3->c0. + */ +#ifndef _VN_MATH_H_ +#define _VN_MATH_H_ + +#include "vn_linearAlgebra.h" +#include "vn_kinematics.h" + +#endif /* _VN_MATH_H_ */ \ No newline at end of file diff --git a/src/vectornav/include/vectornav/vncp_services.h b/src/vectornav/include/vectornav/vncp_services.h new file mode 100755 index 0000000..927b932 --- /dev/null +++ b/src/vectornav/include/vectornav/vncp_services.h @@ -0,0 +1,314 @@ +/** + * \cond INCLUDE_PRIVATE + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to the cross-platform services for the + * VectorNav C/C++ Library. + */ +#ifndef _VNCP_SERVICES_H_ +#define _VNCP_SERVICES_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1500 + /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ + #include "vnint.h" +#else + #include +#endif + +#ifdef WIN32 + + /* Disable some warnings for Visual Studio with -Wall. */ + #if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4668) + #pragma warning(disable:4820) + #pragma warning(disable:4255) + #endif + + #include + + #if defined(_MSC_VER) + #pragma warning(pop) + #endif + +#endif + +#if defined(__linux__) || defined(__QNXNTO__) + #include +#endif + +/* Determine the level of C support. */ +#if __STDC__ + #if defined (__STDC_VERSION__) + #if (__STDC_VERSION__ >= 1999901L) + #define C99 + #endif + #endif +#endif + +/* Define boolean type. */ +#if defined (C99) + #include +#else + #if !defined (__cplusplus) + #if !defined (__GNUC__) + /* _Bool builtin type is included in GCC. */ + /* ISO C Standard: 5.2.5 An object declared as type _Bool is large + * enough to store the values 0 and 1. */ + typedef int8_t _Bool; + #endif + + /* ISO C Standard: 7.16 Boolean type */ + #define bool _Bool + #define true 1 + #define false 0 + #define __bool_true_false_are_defined 1 + #endif +#endif + +#include "vn_errorCodes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define VN_NULL ((void *) 0) + +#ifdef WIN32 + typedef HANDLE VN_HANDLE; + typedef CRITICAL_SECTION VN_CRITICAL_SECTION; +#elif defined(__linux__) || defined(__QNXNTO__) + typedef union { + pthread_t pThreadHandle; + int comPortHandle; + pthread_mutex_t mutexHandle; + void* conditionAndMutexStruct; + } VN_HANDLE; + typedef pthread_mutex_t VN_CRITICAL_SECTION; +#endif + +typedef void *(*VN_THREAD_START_ROUTINE)(void*); + +/** + * \brief Creates a new thread. + * + * \param[out] newThreadHandle Handle of the newly created thread. + * \param[in] startRoutine Pointer to the routine the new thread should execute. + * \param[in] routineData Pointer to data that will be passed to the new thread's execution routine. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_thread_startNew( + VN_HANDLE* newThreadHandle, + VN_THREAD_START_ROUTINE startRoutine, + void* routineData); + +/** + * \brief Opens a COM port. + * + * \param[out] newComPortHandle Handle to the newly opened COM port. + * \param[in] portName The name of the COM port to open. + * \param[in] baudrate The baudrate to communicate at. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_open( + VN_HANDLE* newComPortHandle, + char const* portName, + unsigned int baudrate); + +/** + * \brief Write data out of a COM port. + * + * \param[in] comPortHandle Handle to an open COM port. + * \param[in] dataToWrite Pointer to array of bytes to write out the COM port. + * \param[in] numOfBytesToWrite The number of bytes to write from the dataToWrite pointer. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_writeData( + VN_HANDLE comPortHandle, + char const* dataToWrite, + unsigned int numOfBytesToWrite); + +/** + * \brief Reads data from a COM port. Will block temporarily for a short amount + * of time and then return if no data has been received. + * + * \param[in] comPortHandle Handle to an open COM port. + * \param[out] readBuffer Pointer to a buffer where data read from the COM port will be placed into. + * \param[in] numOfBytesToRead The number of bytes to attempt reading from the COM port. + * \param[out] numOfBytesActuallyRead The number of bytes actually read from the COM port during the read attempt. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_readData( + VN_HANDLE comPortHandle, + char* readBuffer, + unsigned int numOfBytesToRead, + unsigned int* numOfBytesActuallyRead); + +/** + * \brief Closes the COM port. + * + * \param[in] comPortHandle Handle to an open COM port. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_close( + VN_HANDLE comPortHandle); + +/** + * \brief Determines if the COM port needs to be optimized. Currently only + * USB virtual COM ports on Windows need to be optimized for their + * Latency Timer value. + * + * \param[in] portName The COM port name. + * \param[out] isOptimized true if the COM port does not need optimization; + * false if the COM port needs to be optimized. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_isOptimized( + char const* portName, + bool* isOptimized); + +/** + * \brief Attempts to optimize the COM port. Currently only USB virtual COM + * ports on Windows need to be optimized for their Latency Timer value. + * + * \param[in] portName The COM port name. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_comPort_optimize( + char const* portName); + +/** + * \brief Creates a new event. + * + * \param[out] newEventHandle Returns the handle of the newly created event. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_event_create( + VN_HANDLE* newEventHandle); + +/** + * \brief Causes the calling thread to wait on an event until the event is signalled. + * + * \param[in] eventHandle Handle to the event. + * \param[in] timeout The number of milliseconds to wait before the + * thread stops listening. -1 indicates that the wait time is inifinite. If a + * timeout does occur, the value VNERR_TIMEOUT will be retured. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_event_waitFor( + VN_HANDLE eventHandle, + int timeout); + +/** + * \brief Puts the provided event into a signalled state. + * + * \param[in] eventHandle Handle to the event. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_event_signal( + VN_HANDLE eventHandle); + +/** + * \brief Initializes a new critical section object. + * + * \param[out] criticalSection Returns the newly initialized created critical control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_criticalSection_initialize( + VN_CRITICAL_SECTION* criticalSection); + +/** + * \brief Attempt to enter a critical section. + * + * \param[in] criticalSection Pointer to the critical section control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_criticalSection_enter( + VN_CRITICAL_SECTION* criticalSection); + +/** + * \brief Signals that the current executing thread is leaving the critical section. + * + * \param[in] criticalSection Pointer to the critical section control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_criticalSection_leave( + VN_CRITICAL_SECTION* criticalSection); + +/** + * \brief Disposes and frees the resources associated with a critical section control object. + * + * \param[in] criticalSection Pointer to the critical section control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_criticalSection_dispose( + VN_CRITICAL_SECTION* criticalSection); + +/** + * \brief Sleeps the current thread the specified number of milliseconds. + * + * \param[in] numOfMillisecondsToSleep The number of milliseconds to pause the current thread. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vncp_sleepInMs( + unsigned int numOfMillisecondsToSleep); + +/** + * \brief Starts timer for counting a time difference in milliseconds. + */ +void vncp_startMsTimer(); + +/** + * \brief Stops the millisecond timer and returns the difference in milliseconds. + * + * \return The time difference in milliseconds. If the value is -1.0, this + * indicates the vncp_startMsTimer was not called first. + */ +double vncp_stopMsTimer(); + +#ifdef __cplusplus +} +#endif + +#endif /* _VNCP_SERVICES_H_ */ + +/** \endcond */ diff --git a/src/vectornav/include/vectornav/vndevice.h b/src/vectornav/include/vectornav/vndevice.h new file mode 100755 index 0000000..b29da24 --- /dev/null +++ b/src/vectornav/include/vectornav/vndevice.h @@ -0,0 +1,1747 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides access to common functionality amongst VectorNav + * devices. + */ +#ifndef _VNDEVICE_H_ +#define _VNDEVICE_H_ + +/* Disable some unnecessary warnings for compiling using Visual Studio with -Wall. */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4668) /* Preprocessor macro not defined warning. */ +#endif + +#if defined(_MSC_VER) && _MSC_VER <= 1500 + /* Visual Studio 2008 and earlier do not include the stdint.h header file. */ + #include "vnint.h" +#else + #include +#endif + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +#include "vncp_services.h" +#include "vn_kinematics.h" +#include "vn_linearAlgebra.h" + +#if defined(EXPORT_TO_DLL) + #define DLL_EXPORT __declspec(dllexport) +#else + /** Don't compile the library with support for DLL function export. */ + #define DLL_EXPORT +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @defgroup ASYNC_OUTPUTS Asynchronous Output Types + * + * @{ + */ +#define VNASYNC_OFF 0 /**< Asynchronous output is turned off. */ +#define VNASYNC_VNYPR 1 /**< Asynchronous output type is Yaw, Pitch, Roll. */ +#define VNASYNC_VNQTN 2 /**< Asynchronous output type is Quaternion. */ +#define VNASYNC_VNQTM 3 /**< Asynchronous output type is Quaternion and Magnetic. */ +#define VNASYNC_VNQTA 4 /**< Asynchronous output type is Quaternion and Acceleration. */ +#define VNASYNC_VNQTR 5 /**< Asynchronous output type is Quaternion and Angular Rates. */ +#define VNASYNC_VNQMA 6 /**< Asynchronous output type is Quaternion, Magnetic and Acceleration. */ +#define VNASYNC_VNQAR 7 /**< Asynchronous output type is Quaternion, Acceleration and Angular Rates. */ +#define VNASYNC_VNQMR 8 /**< Asynchronous output type is Quaternion, Magnetic, Acceleration and Angular Rates. */ +#define VNASYNC_VNDCM 9 /**< Asynchronous output type is Directional Cosine Orientation Matrix. */ +#define VNASYNC_VNMAG 10 /**< Asynchronous output type is Magnetic Measurements. */ +#define VNASYNC_VNACC 11 /**< Asynchronous output type is Acceleration Measurements. */ +#define VNASYNC_VNGYR 12 /**< Asynchronous output type is Angular Rate Measurements. */ +#define VNASYNC_VNMAR 13 /**< Asynchronous output type is Magnetic, Acceleration, and Angular Rate Measurements. */ +#define VNASYNC_VNYMR 14 /**< Asynchronous output type is Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements. */ +#define VNASYNC_VNYCM 15 /**< Asynchronous output type is Yaw, Pitch, Roll, and Calibrated Measurements. */ +#define VNASYNC_VNYBA 16 /**< Asynchronous output type is Yaw, Pitch, Roll, Body True Acceleration. */ +#define VNASYNC_VNYIA 17 /**< Asynchronous output type is Yaw, Pitch, Roll, Inertial True Acceleration. */ +#define VNASYNC_VNICM 18 /**< Asynchronous output type is Yaw, Pitch, Roll, Inertial Magnetic/Acceleration, and Angular Rate Measurements. */ +#define VNASYNC_VNIMU 19 /**< Asynchronous output type is Calibrated Interial Measurements. */ +#define VNASYNC_VNGPS 20 /**< Asynchronous output type is GPS Measurements. */ +#define VNASYNC_VNINS 22 /**< Asynchronous output type is INS Solution. */ +#define VNASYNC_VNRAW 252 /**< Asynchronous output type is Raw Voltage Measurements. */ +#define VNASYNC_VNCMV 253 /**< Asynchronous output type is Calibrated Measurements. */ +#define VNASYNC_VNSTV 254 /**< Asynchronous output type is Kalman Filter State Vector. */ +#define VNASYNC_VNCOV 255 /**< Asynchronous output type is Kalman Filter Covariance Matrix Diagonal. */ +/** @} */ + +/** + * @defgroup BINARY_ASYNC_MODES Binary Asynchronous Modes + * + * @{ + */ +#define BINARY_ASYNC_MODE_NONE 0 /**< No output of binary async data. */ +#define BINARY_ASYNC_MODE_SERIAL_1 1 /**< Binary messages are sent out serial port 1. */ +#define BINARY_ASYNC_MODE_SERIAL_2 2 /**< Binary messages are sent out serial port 2. */ +#define BINARY_ASYNC_MODE_SERIAL_1_AND_2 3 /**< Binary messages are sent out serial ports 1 and 2. */ +/** @} */ + +/** + * @defgroup BG1_SELECTIONS Binary Group 1 Selections + * + * @{ + */ +#define BG1_NONE 0x0000 /**< No ouput. */ +#define BG1_TIME_STARTUP 0x0001 /**< Time since startup. */ +#define BG1_TIME_GPS 0x0002 /**< GPS time. */ +#define BG1_TIME_SYNC_IN 0x0004 /**< Time since the last SyncIn trigger. */ +#define BG1_YPR 0x0008 /**< Estimated attitude as yaw, pitch, roll in degrees. */ +#define BG1_QTN 0x0010 /**< Estimated attitude as a quaternion. */ +#define BG1_ANGULAR_RATE 0x0020 /**< Compensated angular rate. */ +#define BG1_POSITION 0x0040 /**< Estimated position given as latitude, longitude and altitude. */ +#define BG1_VELOCITY 0x0080 /**< Estimated velocity. */ +#define BG1_ACCEL 0x0100 /**< Estimated acceleration. */ +#define BG1_IMU 0x0200 /**< Calibrated uncompensated angular rate and acceleration measurements. */ +#define BG1_MAG_PRES 0x0400 /**< Calibrated magnetic, temperature and pressure measurements. */ +#define BG1_DELTA_THETA 0x0800 /**< Delta time, theta and velocity. */ +#define BG1_INS_STATUS 0x1000 /**< INS status. */ +#define BG1_SYNC_IN_CNT 0x2000 /**< SyncIn count. */ +/** @} */ + +/** + * @defgroup BG2_SELECTIONS Binary Group 2 Selections + * + * @{ + */ +#define BG2_NONE 0x0000 /**< No output. */ +#define BG2_TIME_STARTUP 0x0001 /**< Time since startup. */ +#define BG2_TIME_GPS 0x0002 /**< Absolute GPS time. */ +#define BG2_GPS_TOW 0x0004 /**< Time since start of GPS week. */ +#define BG2_GPS_WEEK 0x0008 /**< GPS week. */ +#define BG2_TIME_SYNC_IN 0x0010 /**< Time since the last SyncIn trigger. */ +#define BG2_TIME_PPS 0x0020 /**< Time since the last GPS PPS trigger. */ +#define BG2_TIME_UTC 0x0040 /**< UTC time. */ +#define BG2_SYNC_IN_CNT 0x0080 /**< SyncIn trigger count. */ +/** @} */ + +/** + * @defgroup BG3_SELECTIONS Binary Group 3 Selections + * + * @{ + */ +#define BG3_NONE 0x0000 /**< No output. */ +#define BG3_UNCOMP_MAG 0x0002 /**< Uncompensated magnetic measurements. */ +#define BG3_UNCOMP_ACCEL 0x0004 /**< Uncompensated acceleration measurements. */ +#define BG3_UNCOMP_GYRO 0x0008 /**< Uncompensated angular rate measurements. */ +#define BG3_TEMP 0x0010 /**< Temperature measurements. */ +#define BG3_PRES 0x0020 /**< Pressure measurements. */ +#define BG3_DELTA_THETA 0x0040 /**< Delta theta angles. */ +#define BG3_DELTA_V 0x0080 /**< Delta velocity. */ +#define BG3_MAG 0x0100 /**< Compensated magnetic measurements. */ +#define BG3_ACCEL 0x0200 /**< Compensated acceleration measurements. */ +#define BG3_GYRO 0x0400 /**< Compensated angular rate measurements. */ +#define BG3_SENS_SAT 0x0800 /**< Sensor saturation bit field. */ +/** @} */ + +/** + * @defgroup BG4_SELECTIONS Binary Group 4 Selections + * + * @{ + */ +#define BG4_NONE 0x0000 /**< No output. */ +#define BG4_UTC 0x0001 /**< GPS UTC time. */ +#define BG4_TOW 0x0002 /**< GPS time of week. */ +#define BG4_WEEK 0x0004 /**< GPS week. */ +#define BG4_NUM_SATS 0x0008 /**< Number of tracked satellites. */ +#define BG4_FIX 0x0010 /**< GPS fix. */ +#define BG4_POS_LLA 0x0020 /**< GPS position in latitude, longitude and altitude. */ +#define BG4_POS_ECEF 0x0040 /**< GPS position in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG4_VEL_NED 0x0080 /**< GPS velocity in North, East, Down (NED) frame. */ +#define BG4_VEL_ECEF 0x0100 /**< GPS velocity in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG4_POS_U 0x0200 /**< GPS position uncertainty in North, East, Down (NED) frame. */ +#define BG4_VEL_U 0x0400 /**< GPS velocity uncertainty. */ +#define BG4_TIME_U 0x0800 /**< GPS time uncertainty. */ +/** @} */ + +/** + * @defgroup BG5_SELECTIONS Binary Group 5 Selections + * + * @{ + */ +#define BG5_NONE 0x0000 /**< No output. */ +#define BG5_VPE_STATUS 0x0001 /**< VPE status. */ +#define BG5_YPR 0x0002 /**< Yaw, pitch, roll. */ +#define BG5_QUATERNION 0x0004 /**< Quaternion. */ +#define BG5_DCM 0x0008 /**< Directon cosine matrix. */ +#define BG5_MAG_NED 0x0010 /**< Compensated magnetic in North, East, Down (NED) frame. */ +#define BG5_ACCEL_NED 0x0020 /**< Compensated acceleration in North, East, Down (NED) frame. */ +#define BG5_LINEAR_ACCEL_BODY 0x0040 /**< Compensated linear acceleration (no gravity) in the body frame. */ +#define BG5_LINEAR_ACCEL_NED 0x0080 /**< Compensated linear acceleration (no gravity) in the North, East, Down (NED) frame. */ +#define BG5_YPR_U 0x0100 /**< Yaw, pitch, roll uncertainty. */ +/** @} */ + +/** + * @defgroup BG6_SELECTIONS Binary Group 6 Selections + * + * @{ + */ +#define BG6_NONE 0x0000 /**< No output. */ +#define BG6_INS_STATUS 0x0001 /**< INS status. */ +#define BG6_POS_LLA 0x0002 /**< INS position in latitude, longitude and altitude. */ +#define BG6_POS_ECEF 0x0004 /**< INS position in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG6_VEL_BODY 0x0008 /**< INS velocity in body frame. */ +#define BG6_VEL_NED 0x0010 /**< INS velocity in North, East, Down (NED) frame. */ +#define BG6_VEL_ECEF 0x0020 /**< INS velocity in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG6_MAG_ECEF 0x0040 /**< Compensated magnetic in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG6_ACCEL_ECEF 0x0080 /**< Compensated acceleration in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG6_LINEAR_ACCEL_ECEF 0x0100 /**< Compensated linear acceleration (no gravity) in Earth-Centered, Earth-Fixed (ECEF) frame. */ +#define BG6_POS_U 0x0200 /**< INS position uncertainty. */ +#define BG6_VEL_U 0x0400 /**< INS velocity uncertainty. */ +/** @} */ + +#define VN_RESPONSE_MATCH_SIZE 10 /**< Size to match for the response. */ +#define VN_MAX_RESPONSE_SIZE 256 /**< Maximum response size from sensor. */ +#define VN_MAX_COMMAND_SIZE 256 /**< Maximum size of command to send to the sensor. */ + +/** + * \brief Represents a UTC time. + */ +typedef struct { + int8_t year; /**< Year. */ + uint8_t month; /**< Month. */ + uint8_t day; /**< Day. */ + uint8_t hour; /**< Hour. */ + uint8_t min; /**< Minute. */ + uint8_t sec; /**< Second. */ + uint16_t ms; /**< Millisecond. */ +} UtcTime; + +/* Disable some unecessary warnings for compiling using Visual Studio with -Wall. */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4820) /* Padding added in structures warning. */ +#endif + +/** + * \brief Composite structure of the various asynchronous data VectorNav devices + * are capable of outputting. + */ +typedef struct { + VnYpr ypr; /**< Yaw, pitch, roll. */ + VnQuaternion quaternion; /**< Quaternion. */ + VnVector3 magnetic; /**< Magnetic measurements. */ + VnVector3 acceleration; /**< Acceleration measurements. */ + VnVector3 angularRate; /**< Angular rate / gyro measurements. */ + VnMatrix3x3 dcm; /**< Direction cosine matrix. */ + double temperature; /**< Temperature. */ + VnVector3 magneticVoltage; /**< Magnetic sensor voltages. */ + VnVector3 accelerationVoltage; /**< Accleration sensor voltages. */ + VnVector3 angularRateVoltage; /**< Angular rate sensor voltages. */ + double temperatureVoltage; /**< Temperatue sensor voltages. */ + VnVector3 angularRateBias; /**< Angular rate estimated biases. */ + VnVector3 attitudeVariance; /**< Variance for the computed attitude. */ + VnVector3 angularRateBiasVariance; /**< Angular rate bias variance. */ + uint64_t timeStartup; /**< Time since startup. */ + uint64_t timeGps; /**< GPS time. */ + uint64_t timeSyncIn; /**< Time since the last SyncIn trigger. */ + VnVector3 latitudeLongitudeAltitude; /**< The estimated latitude, longitude and altitude. */ + VnVector3 velocity; /**< Velocity measurements. */ + VnVector3 angularRateUncompensated; /**< Uncompensated angular rate measurements. */ + VnVector3 accelerationUncompensated; /**< Uncompensated acceleration measurements. */ + VnVector3 magneticUncompensated; /**< Uncompensated magnetic measurements. */ + double pressure; /**< Pressure measurements. */ + double deltaTime; /**< Time interval that the delta angles and velocities are integrated over. */ + VnVector3 deltaTheta; /**< The delta rotation angles due to rotation since the values were last output by the device. */ + VnVector3 deltaVelocity; /**< The delta velocity due to motion since the values were last output by the device. */ + uint16_t insStatus; /**< Status flags for the INS filter. */ + uint32_t syncInCnt; /**< Number of SyncIn trigger events. */ + uint64_t timeGpsPps; /**< Time since the last GPS PPS trigger event. */ + double gpsTowSec; /**< GPS time of week in seconds. */ + uint64_t gpsTowNs; /**< GPS time of week in nanoseconds. */ + uint16_t gpsWeek; /**< GPS week. */ + UtcTime timeUtc; /**< UTC time. */ + uint16_t sensSat; /**< Flags for identifying sensor saturation. */ + uint8_t numSats; /**< Number of tracked GPS satellites. */ + uint8_t gpsFix; /**< The current GPS fix. */ + VnVector3 gpsPosEcef; /**< The current GPS position given in Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 gpsVelEcef; /**< The current GPS velocity in the Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 gpsPosU; /**< The current GPS position uncertainty in the North, East, Down (NED) frame. */ + double gpsVelU; /**< The current GPS velocity uncertainty in m/s. */ + uint32_t timeU; /**< The current GPS time uncertainty given in nanoseconds. */ + float timeAccSec; /**< The current GPS time accuracy given in seconds. */ + uint16_t vpeStatus; /**< The VPE status bitfield. */ + VnVector3 magNed; /**< The estimated magnetic field in North, East, Down (NED) frame. */ + VnVector3 accelNed; /**< The estimated acceleration (with gravity) given in the North, East, Down (NED) frame. */ + VnVector3 linearAccelBody; /**< The estimated linear acceleration (without gravity) given in the body frame. */ + VnVector3 linearAccelNed; /**< The estimated linear acceleration (without gravity) givein in the North, East, Down (NED) frame. */ + VnVector3 yprU; /**< The estimated attitude (Yaw, Pitch, Roll) uncertainty. */ + VnVector3 velBody; /**< The estimated velocity in the body frame. */ + VnVector3 velNed; /**< The estimated velocity in the North, East, Down (NED) frame. */ + VnVector3 gpsPosLla; /**< The current GPS possition given in latitude, longitude, and altitude. */ + VnVector3 gpsVelocity; /**< The current GPS velocity. */ + VnVector3 posEcef; /**< The estimated position given in Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 velEcef; /**< The estimated velocity given in Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 magEcef; /**< The compensated magnetic measurement givein in Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 accelEcef; /**< The estimated acceleration (with gravity) givin in Earth-Centered, Earth-Fixed (ECEF) frame. */ + VnVector3 linearAccelEcef; /**< The estimated linear acceleration (without gravity) givin in Earth-Centered, Earth-Fixed (ECEF) frame. */ + double posU; /**< The estimated uncertainty of the current position estimate in meters. */ + double velU; /**< The estimated uncertainty of the current velocity estimate in m/s. */ + float attitudeUncertainty; /**< Uncertainty in attitude estimate. */ +} VnDeviceCompositeData; + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +/** + * \brief Fuction type used for receiving notifications of when new asynchronous + * data is received. + * + * \param[in] sender The device that sent the notification. + * \param[in] newData Pointer to the new data. + */ +typedef void (*VnDeviceNewAsyncDataReceivedListener)(void* sender, VnDeviceCompositeData* newData); + +/** + * \brief Function type used for receiving notification of errors received from the sensor. + * + * \param[in] sender The device that sent the notification. + * \param[in] errorCode The error code received from the sensor converted into the library's VN_ERROR_CODE format. + */ +typedef void (*VnDeviceErrorCodeReceivedListener) (void* sender, VN_ERROR_CODE errorCode); + +/* Disable some unecessary warnings for compiling using Visual Studio with -Wall. */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable:4820) /* Padding added in structures warning. */ +#endif + +/** + * Internally used data structure for the VectorNav object. + */ +typedef struct { + + /** + * Handle to the comPortServiceThread. + */ + VN_HANDLE comPortServiceThreadHandle; + + /** + * Handle to the COM port. + */ + VN_HANDLE comPortHandle; + + VN_HANDLE waitForThreadToStopServicingComPortEvent; + VN_HANDLE waitForThreadToStartServicingComPortEvent; + + /** + * Used by the user thread to wait until the comPortServiceThread receives + * a command response. + */ + VN_HANDLE waitForCommandResponseEvent; + + /** + * Critical section for communicating over the COM port. + */ + VN_CRITICAL_SECTION critSecForComPort; + + /** + * Critical section for accessing the response match fields. + */ + VN_CRITICAL_SECTION critSecForResponseMatchAccess; + + /** + * Critical section for accessing the latestAsyncData field. + */ + VN_CRITICAL_SECTION critSecForLatestAsyncDataAccess; + + /** + * Signals to the comPortServiceThread if it should continue servicing the + * COM port. + */ + bool continueServicingComPort; + + /** + * This field is used to signal to the comPortServiceThread that it should + * be checking to a command response comming from the VN-100 device. The + * user thread can toggle this field after setting the cmdResponseMatch + * field so the comPortServiceThread differeniate between the various + * output data of the VN-100. This field should only be accessed in the + * functions vn100_shouldCheckForResponse_threadSafe, + * vn100_enableResponseChecking_threadSafe, and + * vn100_disableResponseChecking_threadSafe. + */ + bool checkForResponse; + + /** + * This field contains the string the comPortServiceThread will use to + * check if a data packet from the VN-100 is a match for the command sent. + * This field should only be accessed in the functions + * vn100_shouldCheckForResponse_threadSafe, vn100_enableResponseChecking_threadSafe, + * and vn100_disableResponseChecking_threadSafe. + */ + char cmdResponseMatchBuffer[VN_RESPONSE_MATCH_SIZE + 1]; + + /** + * This field is used by the comPortServiceThread to place responses + * received from commands sent to the VN-100 device. The responses + * placed in this buffer will be null-terminated and of the form + * "$VNRRG,1,VN-100" where the checksum is stripped since this will + * have already been checked by the thread comPortServiceThread. + */ + char cmdResponseBuffer[VN_MAX_RESPONSE_SIZE + 1]; + + VnDeviceCompositeData lastestAsyncData; + + /** + * This field specifies the number of milliseconds to wait for a response + * from sensor before timing out. + */ + int timeout; + + /** + * Holds pointer to a listener for async data recieved. + */ + VnDeviceNewAsyncDataReceivedListener asyncDataListener; + + /** + * Holds any error code received from the sensor. + */ + VN_ERROR_CODE sensorError; + + /** + * Holds pointer to a listener for error codes received from the VectorNav module. + */ + VnDeviceErrorCodeReceivedListener errorCodeListener; + + /** + * Mask for the end user on what the public struct for this device. + */ + void* deviceMask; + +} VnDevice; + +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + +/* Function declarations. ****************************************************/ + +/** + * \brief Performs a send command and then receive response transaction. + * + * Takes a command of the form "$VNRRG,1" and transmits it to the VectorNav device. + * The function will then wait until the response is received. The response + * will be located in the VnDevice->cmdResponseBuffer field and will be + * null-terminated. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \param[in] responseMatch + * Null-terminated string which will be used by the comPortServiceThread to + * determine if a received data packet is a match for the command sent. + * + * \param[in] cmdToSend + * Pointer to the command data to transmit to the VectorNav device. Should be + * null-terminated. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_transaction( + VnDevice* vndevice, + const char* cmdToSend, + const char* responseMatch); + + +/** + * \brief Allows registering a function which will be called whenever an error + * code is received from the VectorNav module. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] listener The function pointer to be called when an error code + * is received. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_registerErrorCodeReceivedListener( + VnDevice* vndevice, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when error codes from the VectorNav module are received. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_unregisterErrorCodeReceivedListener( + VnDevice* vndevice, + VnDeviceErrorCodeReceivedListener listener); + +/** + * \brief Computes the CRC8 checksum for the provided command. + * + * \param[in] cmdToCheck Null-terminated string of the form "VNRRG,1". + * + * \return The computed checksum number. + */ +DLL_EXPORT uint8_t vndevice_checksum_computeCrc8FromCommand( + const char* cmdToCheck); + +/** + * \brief Computes the CRC16 checksum for the provided command. + * + * \param[in] data The array of data to perform the CRC16 checksum on. + * \param[in] length The number of bytes in the data array to compute the + * checksum over. + * + * \return The computed checksum number. + */ +DLL_EXPORT uint16_t vndevice_checksum_computeCrc16( + const char data[], + uint32_t length); + +/** + * \brief Computes the CRC16 checksum for the provided command. + * + * \param[in] cmdToCheck Null-terminated string of the form "VNRRG,1". + * + * \return The computed checksum number. + */ +DLL_EXPORT uint16_t vndevice_checksum_computeCrc16FromCommand( + const char* cmdToCheck); + +/** + * \brief Computes the checksum for the provided command and returns it as a + * two character string representing it in hexidecimal. + * + * \param[in] cmdToCheck + * Null-terminated string of the form "VNRRG,1". + * + * \param[out] checksum + * A character array of length 2 which the computed checksum will be placed. + */ +DLL_EXPORT void vndevice_checksum_computeAndReturnAsHex( + const char* cmdToCheck, + char* checksum); + +/** + * \brief Computes the expected length of a received binary packet. + * + * \param[in] groupIndex The current index of the group field settings. + * \param[in] groupField The value of the group field. + * + * \return Unknown. + */ +int vndevice_computeLengthOfBinaryGroupPayload( + unsigned char groupIndex, + uint16_t groupField); + +/** + * \brief Process a received and verified binary packet. + * + * \param[in] vndevice Pointer to the VnDevice object. + * \param[in] buffer Pointer to the buffer containing the start of the received binary packet. + */ +void vndevice_processReceivedBinaryPacket( + VnDevice* vndevice, + char* buffer); + +/** + * \brief Provides initialization to the VnDevice data structure. + * + * \param[in] vndevice Pointer to an uninitialized VnDevice structure. + * \param[in] portName The name of the port to connect to. + * \param[in] baudrate The baudrate to connect at. + * \param[in] deviceMask Used to mimic the top-level device structure (i.e. Vn100 or Vn200). + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_initializeVnDevice( + VnDevice* vndevice, + const char* portName, + int baudrate, + void* deviceMask); + +/** + * \brief Deinitalized the VnDevice object. + * + * \param[in] vndevice Pointer to the VnDevice structure to deinitialize. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_deinitializeVnDevice( + VnDevice* vndevice); + +/** + * \brief Allows registering a function which will be called whenever a new + * asynchronous data packet is received from the VectorNav module. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] listener The function pointer to be called when asynchronous data + * is received. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_registerAsyncDataReceivedListener( + VnDevice* vndevice, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Unregisters an already registered function for recieving notifications + * of when new asynchronous data is received. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] listener The function pointer to unregister. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_unregisterAsyncDataReceivedListener( + VnDevice* vndevice, + VnDeviceNewAsyncDataReceivedListener listener); + +/** + * \brief Commands the VnDevice control object to start processing communcation. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_startHandlingCommunication( + VnDevice* vndevice); + +/** + * \brief Waits for the communication handler thread to start processing data. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_waitForThreadToStartHandlingCommunicationPort( + VnDevice* vndevice); + +/** + * \brief Writes out the provided command to the COM port. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] cmdToSend Pointer to the command to send out. + * + * \return VectorNav error code. + */ +VN_ERROR_CODE vndevice_writeOutCommand( + VnDevice* vndevice, + const char* cmdToSend); + +/** + * \brief Retrieves a pointer to the VnDevice response buffer. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \return The pointer to the response buffer. + */ +char* vndevice_getResponseBuffer( + VnDevice* vndevice); + +/** + * \brief Gets the current configuration of the requested binary output register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to query for its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] selectedOutputGroups The selected output groups. + * \param[out] outputGroup1Selections The configured output data types for output group 1. + * \param[out] outputGroup2Selections The configured output data types for output group 2. + * \param[out] outputGroup3Selections The configured output data types for output group 3. + * \param[out] outputGroup4Selections The configured output data types for output group 4. + * \param[out] outputGroup5Selections The configured output data types for output group 5. + * \param[out] outputGroup6Selections The configured output data types for output group 6. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getBinaryOutputConfiguration( + VnDevice* vndevice, + uint8_t binaryOutputRegisterId, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections); + +/** + * \brief Sets the configuration of the requested binary output register. Note + * that you do not have to provide the selected output groups option since this + * will be determined from the provided configurations for the individual output + * groups. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] binaryOutputRegisterId The ID of the binary output register to set its configuration. Must be 1 - 3. + * \param[out] asyncMode The selected asyncMode for the binary output register. + * \param[out] rateDivisor The rate divisor. + * \param[out] outputGroup1Selections The output data types for output group 1. + * \param[out] outputGroup2Selections The output data types for output group 2. + * \param[out] outputGroup3Selections The output data types for output group 3. + * \param[out] outputGroup4Selections The output data types for output group 4. + * \param[out] outputGroup5Selections The output data types for output group 5. + * \param[out] outputGroup6Selections The output data types for output group 6. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setBinaryOutputConfiguration( + VnDevice* vndevice, + uint8_t binaryOutputRegisterId, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse); + + +/** + * \brief Retrieves the associated timeout value for the VnDevice object. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \return The timeout value in milliseconds. -1 indicates that timeouts are + * not used. + */ +DLL_EXPORT int vndevice_get_timeout( + VnDevice* vndevice); + +/** + * \brief Sets the timeout value for the reading values from the VectorNav sensor. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] timeout The timeout value in milliseconds. Specify -1 to not use + * any timeouts. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_set_timeout( + VnDevice* vndevice, + int timeout); + +/** + * \brief Retrieves the most recent stored asynchronous data. + * + * \param[in] vndevice Pointer to the VnDevice object. + * \param[out] curData Returned pointer current asychronous data. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getCurrentAsyncData( + VnDevice* vndevice, + VnDeviceCompositeData* curData); + +/** + * \brief Commands the VectorNav unit to write its current register setting to + * non-volatile memory. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_writeSettings( + VnDevice* vndevice, + bool waitForResponse); + +/** + * \brief Commands the VectorNav unit to revert its settings to factory defaults. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_restoreFactorySettings( + VnDevice* vndevice, + bool waitForResponse); + +/** + * \brief Commands the VectorNav module to reset itself. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_reset( + VnDevice* vndevice); + +/** + * \brief Gets the values in the User Tag register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] userTagBuffer Buffer to store the response. Must have a length of at least 21 characters. + * \param[in] userTagBufferLength Length of the provided userTagBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getUserTag( + VnDevice* vndevice, + char* userTagBuffer, + uint32_t userTagBufferLength); + +/** + * \brief Sets the values of the User Tag register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] userTagData Array containg the data to send. Length must be equal to or less than 20 characters. + * \param[in] userTagDataLength Length of the data to send in the userTagData array. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setUserTag( + VnDevice* vndevice, + char* userTagData, + uint32_t userTagDataLength, + bool waitForResponse); + +/** + * \brief Gets the values in the Model Number register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] modelBuffer Buffer to store the response. Must have a length of at least 25 characters. + * \param[in] modelBufferLength Length of the provided modelBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getModelNumber( + VnDevice* vndevice, + char* modelBuffer, + uint32_t modelBufferLength); + +/** + * \brief Gets the values in the Hardware Revision register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] hardwareRevision The hardware revision value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getHardwareRevision( + VnDevice* vndevice, + int32_t* hardwareRevision); + +/** + * \brief Gets the values in the Serial Number register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] serialNumberBuffer Buffer to store the response. Must have a length of at least 13 characters. + * \param[in] serialNumberBufferLength Length of the provided serialNumberBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getSerialNumber( + VnDevice* vndevice, + char* serialNumberBuffer, + uint32_t serialNumberBufferLength); + +/** + * \brief Gets the value in the Firmware Version register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] firmwareVersionBuffer Buffer to store the response. Must have a length of at least 16 characters. + * \param[in] firmwareVersionBufferLength Length of the provided firmwareVersionBuffer buffer. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getFirmwareVersion( + VnDevice* vndevice, + char* firmwareVersionBuffer, + uint32_t firmwareVersionBufferLength); + +/** + * \brief Gets the values in the Serial Baud Rate register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] serialBaudrate The serial baudrate value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getSerialBaudRate( + VnDevice* vndevice, + uint32_t* serialBaudrate); + +/** + * \brief Sets the values of the Serial Baud Rate register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] serialBaudrate Value for the serial baudrate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setSerialBaudRate( + VnDevice* vndevice, + uint32_t serialBaudrate, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Type register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] asyncDataOutputType The asynchronous data output type value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getAsynchronousDataOutputType( + VnDevice* vndevice, + uint32_t* asyncDataOutputType); + +/** + * \brief Sets the values of the Asynchronous Data Output Type register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] asyncDataOutputType Value for the asynchronous data output type field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setAsynchronousDataOutputType( + VnDevice* vndevice, + uint32_t asyncDataOutputType, + bool waitForResponse); + +/** + * \brief Gets the values in the Asynchronous Data Output Frequency register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] asyncDataOutputFrequency The asynchronous data output frequency value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getAsynchronousDataOutputFrequency( + VnDevice* vndevice, + uint32_t* asyncDataOutputFrequency); + +/** + * \brief Sets the values of the Asynchronous Data Output Frequency register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] asyncDataOutputFrequency Value for the asynchronous data output frequency field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setAsynchronousDataOutputFrequency( + VnDevice* vndevice, + uint32_t asyncDataOutputFrequency, + bool waitForResponse); + +/** + * \brief Gets the values in the Yaw Pitch Roll register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getYawPitchRoll( + VnDevice* vndevice, + VnYpr* attitude); + +/** + * \brief Gets the values in the Attitude Quaternion register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor Quaterion values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getQuaternion( + VnDevice* vndevice, + VnQuaternion* attitude); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, Magentic, Accleration, and Angular Rates register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getYawPitchRollMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Quaternion, Magnetic, Acceleration and Angular + * Rates register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor Quaterion values. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getQuaternionMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic Measurements register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getMagnetic( + VnDevice* vndevice, + VnVector3* magnetic); + +/** + * \brief Gets the values in the Acceleration Measurements register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getAcceleration( + VnDevice* vndevice, + VnVector3* acceleration); + +/** + * \brief Gets the values in the Angular Rate Measurements register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getAngularRate( + VnDevice* vndevice, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Magnetic, Acceleration and Angular Rates register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] magnetic The current sensor magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Body Acceleration and Angular Rates register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] bodyAcceleration The current sensor body acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getYawPitchRollTrueBodyAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* bodyAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the Yaw,Pitch,Roll, True Inertial Acceleration and Angular Rates register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] attitude The current sensor YawPitchRoll values. + * \param[out] inertialAcceleration The current sensor inertial acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor angular rate (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getYawPitchRollTrueInertialAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* inertialAcceleration, + VnVector3* angularRate); + +/** + * \brief Gets the values in the VPE Basic Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] enable The enable/disable value of the sensor. + * \param[out] headingMode The heading mode value of the sensor. + * \param[out] filteringMode The filtering mode value of the sensor. + * \param[out] tuningMode The tuning mode value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getVpeControl( + VnDevice* vndevice, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode); + +/** + * \brief Sets the values of the VPE Basic Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] enable Value for the enable/disable field. + * \param[in] headingMode Value for the heading mode field. + * \param[in] filteringMode Value for the filtering mode field. + * \param[in] tuningMode Value for the tuning mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setVpeControl( + VnDevice* vndevice, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Magnetometer Basic Tuning register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] baseTuning The current sensor magnetometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor magnetometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor magnetometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getVpeMagnetometerBasicTuning( + VnDevice* vndevice, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Magnetometer Basic Tuning register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] baseTuning The magnetometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The magnetometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The magnetometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setVpeMagnetometerBasicTuning( + VnDevice* vndevice, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the VPE Accelerometer Basic Tuning register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] baseTuning The current sensor accelerometer base tuning (X,Y,Z) values. + * \param[out] adaptiveTuning The current sensor accelerometer adaptive tuning (X,Y,Z) values. + * \param[out] adaptiveFiltering The current sensor accelerometer adaptive filtering (X,Y,Z) values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getVpeAccelerometerBasicTuning( + VnDevice* vndevice, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering); + +/** + * \brief Sets the values of the VPE Accelerometer Basic Tuning register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] baseTuning The accelerometer base tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveTuning The accelerometer adaptive tuning (X,Y,Z) values to write to the sensor. + * \param[in] adaptiveFiltering The accelerometer adaptive filtering (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setVpeAccelerometerBasicTuning( + VnDevice* vndevice, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse); + +/** + * \brief Gets the values in the IMU Measurements register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] magnetic The current sensor uncompensated magnetic (X,Y,Z) values. + * \param[out] acceleration The current sensor uncompensated acceleration (X,Y,Z) values. + * \param[out] angularRate The current sensor uncompensated angular rate (X,Y,Z) values. + * \param[out] temperature The temperature value of the sensor. + * \param[out] pressure The pressure value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getImuMeasurements( + VnDevice* vndevice, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate, + float* temperature, + float* pressure); + +/** + * \brief Gets the values in the Reference Frame Rotation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] c The current sensor C matrix values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getReferenceFrameRotation( + VnDevice* vndevice, + VnMatrix3x3* c); + +/** + * \brief Sets the values of the Reference Frame Rotation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setReferenceFrameRotation( + VnDevice* vndevice, + VnMatrix3x3 c, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] syncInMode The input signal synchronization mode value of the sensor. + * \param[out] syncInEdge The input signal synchronization edge selection value of the sensor. + * \param[out] syncInSkipFactor The input signal trigger skip factor value of the sensor. + * \param[out] syncOutMode The output signal synchronization mode value of the sensor. + * \param[out] syncOutPolarity The output signal synchronization polarity value of the sensor. + * \param[out] syncOutSkipFactor The output synchronization signal skip factor value of the sensor. + * \param[out] syncOutPulseWidth The output synchronization signal pulse width value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getSynchronizationControl( + VnDevice* vndevice, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth); + +/** + * \brief Sets the values of the Synchronization Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] syncInMode Value for the input signal synchronization mode field. + * \param[in] syncInEdge Value for the input signal synchronization edge selection field. + * \param[in] syncInSkipFactor Value for the input signal trigger skip factor field. + * \param[in] syncOutMode Value for the output signal synchronization mode field. + * \param[in] syncOutPolarity Value for the output signal synchronization polarity field. + * \param[in] syncOutSkipFactor Value for the output synchronization signal skip factor field. + * \param[in] syncOutPulseWidth Value for the output synchronization signal pulse width field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setSynchronizationControl( + VnDevice* vndevice, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + bool waitForResponse); + +/** + * \brief Gets the values in the Synchronization Status register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] syncInCount The synchronization in count value of the sensor. + * \param[out] syncInTime The synchronization in time value of the sensor. + * \param[out] syncOutCount The synchronization out count value of the sensor. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getSynchronizationStatus( + VnDevice* vndevice, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount); + +/** + * \brief Sets the values of the Synchronization Status register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] syncInCount Value for the synchronization in count field. + * \param[in] syncInTime Value for the synchronization in time field. + * \param[in] syncOutCount Value for the synchronization out count field. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setSynchronizationStatus( + VnDevice* vndevice, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount, + bool waitForResponse); + +/** + * \brief Gets the contents of the Delta Theta and Delta Velocity register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] deltaTime Delta time for the integration interval. + * \param[out] deltaTheta Delta rotation vector. + * \param[out] deltaVelocity Delta velocity vector. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getDeltaThetaAndDeltaVelocity( + VnDevice* vndevice, + float* deltaTime, + VnVector3* deltaTheta, + VnVector3* deltaVelocity); + +/** + * \brief Gets the values in the Acceleration Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getAccelerationCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Acceleration Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setAccelerationCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetic Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getMagneticCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Magnetic Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setMagneticCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Gets the values in the Gyro Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getGyroCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Sets the values of the Gyro Compensation register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] c The C matrix values to write to the sensor. + * \param[in] b The B vector values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setGyroCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse); + +/** + * \brief Retreives the current values of the IMU Filtering Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[out] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[out] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[out] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[out] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[out] magFilterMode Filtering mode for magnetic measurements. + * \param[out] accelFilterMode Filtering mode for acceleration measurements. + * \param[out] gyroFilterMode Filtering mode for gyro measurements. + * \param[out] tempFilterMode Filtering mode for temperature measurements. + * \param[out] presFilterMode Filtering mode for pressure measurements. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getImuFilteringConfiguration( + VnDevice* vndevice, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode); + +/** + * \brief Sets the values of the IMU Filtering Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] magWindowSize Number of previous measurements averaged for magnetic measurements. + * \param[in] accelWindowSize Number of previous measurements averaged for acceleration measurements. + * \param[in] gyroWindowSize Number of previous measurements averaged for gyro measurements. + * \param[in] tempWindowSize Number of previous measurements averaged for temperature measurements. + * \param[in] presWindowSize Number of previous measurements averaged for pressure measurements. + * \param[in] magFilterMode Filtering mode for magnetic measurements. + * \param[in] accelFilterMode Filtering mode for acceleration measurements. + * \param[in] gyroFilterMode Filtering mode for gyro measurements. + * \param[in] tempFilterMode Filtering mode for temperature measurements. + * \param[in] presFilterMode Filtering mode for pressure measurements. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setImuFilteringConfiguration( + VnDevice* vndevice, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode, + bool waitForResponse); + +/** + * \brief Retreives the current values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] integrationFrame Output frame for delta velocity quantities. + * \param[out] gyroCompensation Compensation to apply to angular rate. + * \param[out] accelCompensation Compensation to apply to accelerations. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getDeltaThetaAndDeltaVelocityConfiguration( + VnDevice* vndevice, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation); + +/** + * \brief Sets the values of the Delta Theta and Delta Velocity Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] integrationFrame Output frame for delta velocity quantities. + * \param[in] gyroCompensation Compensation to apply to angular rate. + * \param[in] accelCompensation Compensation to apply to accelerations. + * \param[in] waitForResponse Signals if the function should block until a response is + received from the sensor. TRUE to block for a response; FALSE to immediately + return after sending out the command. + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setDeltaThetaAndDeltaVelocityConfiguration( + VnDevice* vndevice, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + bool waitForResponse); + +/** + * \brief Gets the values in the Magnetometer Calibration Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] hsiMode The HSIMode value of the sensor. + * \param[out] hsiOutput The HSIOutput value of the sensor. + * \param[out] convergeRate The ConvergeRate value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getMagnetometerCalibrationControl( + VnDevice* vndevice, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate); + +/** + * \brief Sets the values of the Magnetometer Calibration Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] hsiMode Value for the HSIMode field. + * \param[in] hsiOutput Value for the HSIOutput field. + * \param[in] convergeRate Value for the ConvergeRate field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setMagnetometerCalibrationControl( + VnDevice* vndevice, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate, + bool waitForResponse); + +/** + * \brief Gets the values in the Calculated Magnetometer Calibration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] c The current sensor C matrix values. + * \param[out] b The current sensor B vector values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getCalculatedMagnetometerCalibration( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b); + +/** + * \brief Gets the values in the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] magneticReference The current sensor magnetic reference vector (X,Y,Z) values. + * \param[out] gravityReference The current sensor gravity reference vector (X,Y,Z) values. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getMagneticGravityReferenceVectors( + VnDevice* vndevice, + VnVector3* magneticReference, + VnVector3* gravityReference); + +/** + * \brief Sets the values of the Magnetic and Gravity Reference Vectors register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] magneticReference The magnetic reference vector (X,Y,Z) values to write to the sensor. + * \param[in] gravityReference The gravity reference vector (X,Y,Z) values to write to the sensor. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setMagneticGravityReferenceVectors( + VnDevice* vndevice, + VnVector3 magneticReference, + VnVector3 gravityReference, + bool waitForResponse); + +/** + * \brief Gets the values in the Communication Protocol Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] serialCount The serial count value of the sensor. + * \param[out] serialStatus The serial status value of the sensor. + * \param[out] spiCount The SPI count value of the sensor. + * \param[out] spiStatus The SPI status value of the sensor. + * \param[out] serialChecksum The serial checksum value of the sensor. + * \param[out] spiChecksum The SPI checksum value of the sensor. + * \param[out] errorMode The error mode value of the sensor. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getCommunicationProtocolControl( + VnDevice* vndevice, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode); + +/** + * \brief Sets the values of the Communication Protocol Control register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] serialCount Value for the serial count field. + * \param[in] serialStatus Value for the serial status field. + * \param[in] spiCount Value for the SPI count field. + * \param[in] spiStatus Value for the SPI status field. + * \param[in] serialChecksum Value for the serial checksum field. + * \param[in] spiChecksum Value for the SPI checksum field. + * \param[in] errorMode Value for the error mode field. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setCommunicationProtocolControl( + VnDevice* vndevice, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode, + bool waitForResponse); + +/** + * \brief Gets the values in the Reference Vector Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[out] useMagModel The UseMagModel field. + * \param[out] useGravityModel The UseGravityModel field. + * \param[out] recalcThreshold The RecalcThreshold field. + * \param[out] year The Year field. + * \param[out] lla The Lattitude, Longitude, Altitude fields. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_getReferenceVectorConfiguration( + VnDevice* vndevice, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint32_t* recalcThreshold, + float* year, + VnVector3* lla); + +/** + * \brief Sets the values in the Reference Vector Configuration register. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] useMagModel The UseMagModel field. + * \param[in] useGravityModel The UseGravityModel field. + * \param[in] recalcThreshold The RecalcThreshold field. + * \param[in] year The Year field. + * \param[in] lla The Lattitude, Longitude, Altitude fields. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_setReferenceVectorConfiguration( + VnDevice* vndevice, + uint8_t useMagModel, + uint8_t useGravityModel, + uint32_t recalcThreshold, + float year, + VnVector3 lla, + bool waitForResponse); + +/** + * \brief Pauses the asynchronous data output. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_pauseAsyncOutputs( + VnDevice* vndevice, + bool waitForResponse); + +/** + * \brief Resumes the asynchronous data output. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * \param[in] waitForResponse Signals if the function should block until a response is + * received from the sensor. TRUE to block for a response; FALSE to immediately + * return after sending out the command. + * + * \return VectorNav error code. + */ +DLL_EXPORT VN_ERROR_CODE vndevice_resumeAsyncOutputs( + VnDevice* vndevice, + bool waitForResponse); + +#ifdef __cplusplus +} +#endif + +#endif /* _VNDEVICE_H_ */ diff --git a/src/vectornav/include/vectornav/vnint.h b/src/vectornav/include/vectornav/vnint.h new file mode 100755 index 0000000..54fe642 --- /dev/null +++ b/src/vectornav/include/vectornav/vnint.h @@ -0,0 +1,43 @@ +/** + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This header file provides definitions for standard integer types since + * Visual Studio 2008 and earlier do not include the stdint.h header file. + */ +#ifndef _VNINT_H_ +#define _VNINT_H_ + +typedef signed __int8 int8_t; +typedef signed __int16 int16_t; +typedef signed __int32 int32_t; +typedef signed __int64 int64_t; +typedef unsigned __int8 uint8_t; +typedef unsigned __int16 uint16_t; +typedef unsigned __int32 uint32_t; +typedef unsigned __int64 uint64_t; + +#endif /* _VNINT_H_ */ diff --git a/src/vectornav/package.xml b/src/vectornav/package.xml new file mode 100644 index 0000000..73ef20e --- /dev/null +++ b/src/vectornav/package.xml @@ -0,0 +1,19 @@ + + + vectornav + 0.0.0 + VN-100 + + + + + Dexter Watkins + + + + + BSD + catkin + + + diff --git a/src/vectornav/src/vectornav/vn100.c b/src/vectornav/src/vectornav/vn100.c new file mode 100755 index 0000000..de390ea --- /dev/null +++ b/src/vectornav/src/vectornav/vn100.c @@ -0,0 +1,3133 @@ +/** + * \cond INCLUDE_PRIVATE + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This file implements the functions for interfacing with a VN-100 device. + */ +#include +#include +#include +#include "vn100.h" +#include "vn_errorCodes.h" + +/* Defines and constants. ****************************************************/ + +/* Private type definitions. *************************************************/ + +/* Private function definitions. *********************************************/ + +/* Function definitions. *****************************************************/ + +VN_ERROR_CODE vn100_connect( + Vn100* newVn100, + const char* portName, + int baudrate) +{ + VN_ERROR_CODE errorCode; + + newVn100->portName = (char*) malloc(strlen(portName) + 1); + strcpy(newVn100->portName, portName); + newVn100->baudRate = baudrate; + newVn100->isConnected = false; + + errorCode = vndevice_initializeVnDevice(&newVn100->vndevice, portName, baudrate, newVn100); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vndevice_startHandlingCommunication(&newVn100->vndevice); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + newVn100->isConnected = true; + + errorCode = vndevice_waitForThreadToStartHandlingCommunicationPort(&newVn100->vndevice); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_disconnect( + Vn100* vn100) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + vndevice_deinitializeVnDevice(&vn100->vndevice); + + /* Free the memory associated with the Vn100 structure. */ + free(vn100->portName); + + vn100->isConnected = false; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getBinaryOutputConfiguration( + Vn100* vn100, + uint8_t binaryOutputRegisterId, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections) +{ + uint16_t outputGroup2SelectionsPlaceholder, outputGroup4SelectionsPlaceholder, outputGroup6SelectionsPlaceholder; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getBinaryOutputConfiguration( + &vn100->vndevice, + binaryOutputRegisterId, + asyncMode, + rateDivisor, + selectedOutputGroups, + outputGroup1Selections, + &outputGroup2SelectionsPlaceholder, + outputGroup3Selections, + &outputGroup4SelectionsPlaceholder, + outputGroup5Selections, + &outputGroup6SelectionsPlaceholder); +} + +VN_ERROR_CODE vn100_getBinaryOutput1Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections) +{ + return vn100_getBinaryOutputConfiguration( + vn100, + 1, + asyncMode, + rateDivisor, + selectedOutputGroups, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections); +} + +VN_ERROR_CODE vn100_getBinaryOutput2Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections) +{ + return vn100_getBinaryOutputConfiguration( + vn100, + 2, + asyncMode, + rateDivisor, + selectedOutputGroups, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections); +} + +VN_ERROR_CODE vn100_getBinaryOutput5Configuration( + Vn100* vn100, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup5Selections) +{ + return vn100_getBinaryOutputConfiguration( + vn100, + 5, + asyncMode, + rateDivisor, + selectedOutputGroups, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections); +} + +VN_ERROR_CODE vn100_setBinaryOutputConfiguration( + Vn100* vn100, + uint8_t binaryOutputRegisterId, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setBinaryOutputConfiguration( + &vn100->vndevice, + binaryOutputRegisterId, + asyncMode, + rateDivisor, + outputGroup1Selections, + 0, + outputGroup3Selections, + 0, + outputGroup5Selections, + 0, + waitForResponse); + +} + +VN_ERROR_CODE vn100_setBinaryOutput1Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse) +{ + return vn100_setBinaryOutputConfiguration( + vn100, + 1, + asyncMode, + rateDivisor, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections, + waitForResponse); +} + +VN_ERROR_CODE vn100_setBinaryOutput2Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse) +{ + return vn100_setBinaryOutputConfiguration( + vn100, + 2, + asyncMode, + rateDivisor, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections, + waitForResponse); +} + +VN_ERROR_CODE vn100_setBinaryOutput3Configuration( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse) +{ + return vn100_setBinaryOutputConfiguration( + vn100, + 3, + asyncMode, + rateDivisor, + outputGroup1Selections, + outputGroup3Selections, + outputGroup5Selections, + waitForResponse); +} + +VN_ERROR_CODE vn100_tare( + Vn100* vn100, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNTAR"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, "VNTAR"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vn100_knownMagneticDisturbance( + Vn100* vn100, + bool isDisturbancePresent, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + cmdToSend = isDisturbancePresent ? "$VNKMD,1" : "$VNKMD,0"; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, "VNKMD,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vn100_knownAccelerationDisturbance( + Vn100* vn100, + bool isDisturbancePresent, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + cmdToSend = isDisturbancePresent ? "$VNKAD,1" : "$VNKAD,0"; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, "VNKAD,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vn100_setGyroBias( + Vn100* vn100, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNSGB"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, "VNSGB"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vn100_registerAsyncDataReceivedListener( + Vn100* vn100, + VnDeviceNewAsyncDataReceivedListener listener) +{ + return vndevice_registerAsyncDataReceivedListener(&vn100->vndevice, listener); +} + +VN_ERROR_CODE vn100_unregisterAsyncDataReceivedListener( + Vn100* vn100, + VnDeviceNewAsyncDataReceivedListener listener) +{ + return vndevice_unregisterAsyncDataReceivedListener(&vn100->vndevice, listener); +} + +VN_ERROR_CODE vn100_registerErrorCodeReceivedListener( + Vn100* vn100, + VnDeviceErrorCodeReceivedListener listener) +{ + return vndevice_registerErrorCodeReceivedListener(&vn100->vndevice, listener); +} + +VN_ERROR_CODE vn100_unregisterErrorCodeReceivedListener( + Vn100* vn100, + VnDeviceErrorCodeReceivedListener listener) +{ + return vndevice_unregisterErrorCodeReceivedListener(&vn100->vndevice, listener); +} + +bool vn100_verifyConnectivity( + Vn100* vn100) +{ + const char* cmdToSend = "$VNRRG,1"; + const char* responseMatch = "VNRRG,"; + const char* responseMatch1 = "VNRRG,01,VN-100"; + const char* responseMatch2 = "VNRRG,1,VN-100"; + char modelBuffer[25]; + int errorCode; + + if (!vn100->isConnected) + return false; + + memset(modelBuffer, 0, 25); + memset(vndevice_getResponseBuffer(&vn100->vndevice), 0, VN_MAX_RESPONSE_SIZE + 1); + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return false; + + if (strncmp(vndevice_getResponseBuffer(&vn100->vndevice), responseMatch1, strlen(responseMatch1)) == 0) + return true; + + if (strncmp(vndevice_getResponseBuffer(&vn100->vndevice), responseMatch2, strlen(responseMatch2)) == 0) + return true; + + return false; +} + + +VN_ERROR_CODE vn100_getQuaternionMagnetic( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic) +{ + const char* cmdToSend = "$VNRRG,10"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getQuaternionAcceleration( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* acceleration) +{ + const char* cmdToSend = "$VNRRG,11"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getQuaternionAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,12"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getQuaternionMagneticAcceleration( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration) +{ + const char* cmdToSend = "$VNRRG,13"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getQuaternionAccelerationAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,14"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getDirectionCosineMatrix( + Vn100* vn100, + VnMatrix3x3* attitude) +{ + const char* cmdToSend = "$VNRRG,16"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->c22 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getFilterMeasurementVarianceParameters( + Vn100* vn100, + double* angularWalkVariance, + VnVector3* angularRateVariance, + VnVector3* magneticVariance, + VnVector3* accelerationVariance) +{ + const char* cmdToSend = "$VNRRG,22"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *angularWalkVariance = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateVariance->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateVariance->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateVariance->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticVariance->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticVariance->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticVariance->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerationVariance->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerationVariance->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerationVariance->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setFilterMeasurementVarianceParameters( + Vn100* vn100, + double angularWalkVariance, + VnVector3 angularRateVariance, + VnVector3 magneticVariance, + VnVector3 accelerationVariance, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,22,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", angularWalkVariance); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", angularRateVariance.c0, angularRateVariance.c1, angularRateVariance.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", magneticVariance.c0, magneticVariance.c1, magneticVariance.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", accelerationVariance.c0, accelerationVariance.c1, accelerationVariance.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getFilterActiveTuningParameters( + Vn100* vn100, + double* magneticGain, + double* accelerationGain, + double* magneticMemory, + double* accelerationMemory) +{ + const char* cmdToSend = "$VNRRG,24"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magneticGain = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelerationGain = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magneticMemory = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelerationMemory = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setFilterActiveTuningParameters( + Vn100* vn100, + double magneticGain, + double accelerationGain, + double magneticMemory, + double accelerationMemory, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,24,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", magneticGain); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", accelerationGain); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", magneticMemory); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", accelerationMemory); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getAccelerometerGain( + Vn100* vn100, + unsigned int* accelerometerGain) +{ + const char* cmdToSend = "$VNRRG,28"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelerometerGain = (unsigned int) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setAccelerometerGain( + Vn100* vn100, + unsigned int accelerometerGain, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,28,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", accelerometerGain); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_setCommunicationProtocolStatus( + Vn100* vn100, + unsigned int numOfParsedSerialMessages, + unsigned int numOfParsedSpiMessages, + unsigned char maxUsageSerialRxBuffer, + unsigned char maxUsageSerialTxBuffer, + unsigned char maxUsageSpiRxBuffer, + unsigned char maxUsageSpiTxBuffer, + unsigned short systemError0, + unsigned short systemError1, + unsigned short systemError2, + unsigned short systemError3, + unsigned short systemError4, + unsigned short systemError5, + unsigned short systemError6, + unsigned short systemError7, + unsigned short systemError8, + unsigned short systemError9, + unsigned short systemError10, + unsigned short systemError11, + unsigned short systemError12, + unsigned short systemError13, + unsigned short systemError14, + unsigned short systemError15, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,31,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", numOfParsedSerialMessages); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", numOfParsedSpiMessages); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", maxUsageSerialRxBuffer); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", maxUsageSerialTxBuffer); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", maxUsageSpiRxBuffer); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", maxUsageSpiTxBuffer); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError0); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError1); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError3); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError4); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError5); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError6); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError7); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError8); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError9); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError10); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError11); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError12); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError13); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError14); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", systemError15); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getFilterBasicControl( + Vn100* vn100, + unsigned char* magneticMode, + unsigned char* externalMagnetometerMode, + unsigned char* externalAccelerometerMode, + unsigned char* externalGyroscopeMode, + VnVector3* angularRateLimit) +{ + const char* cmdToSend = "$VNRRG,34"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magneticMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *externalMagnetometerMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *externalAccelerometerMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *externalGyroscopeMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateLimit->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateLimit->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRateLimit->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setFilterBasicControl( + Vn100* vn100, + unsigned char magneticMode, + unsigned char externalMagnetometerMode, + unsigned char externalAccelerometerMode, + unsigned char externalGyroscopeMode, + VnVector3 angularRateLimit, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,34,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", magneticMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", externalMagnetometerMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", externalAccelerometerMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", externalGyroscopeMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", angularRateLimit.c0, angularRateLimit.c1, angularRateLimit.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getVpeMagnetometerAdvancedTuning( + Vn100* vn100, + VnVector3* minimumFiltering, + VnVector3* maximumFiltering, + float* maximumAdaptRate, + float* disturbanceWindow, + float* maximumTuning) +{ + const char* cmdToSend = "$VNRRG,37"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *maximumAdaptRate = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *disturbanceWindow = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *maximumTuning = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setVpeMagnetometerAdvancedTuning( + Vn100* vn100, + VnVector3 minimumFiltering, + VnVector3 maximumFiltering, + float maximumAdaptRate, + float disturbanceWindow, + float maximumTuning, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,37,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", minimumFiltering.c0, minimumFiltering.c1, minimumFiltering.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", maximumFiltering.c0, maximumFiltering.c1, maximumFiltering.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", maximumAdaptRate); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", disturbanceWindow); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", maximumTuning); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getVpeAccelerometerAdvancedTuning( + Vn100* vn100, + VnVector3* minimumFiltering, + VnVector3* maximumFiltering, + float* maximumAdaptRate, + float* disturbanceWindow, + float* maximumTuning) +{ + const char* cmdToSend = "$VNRRG,39"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + minimumFiltering->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + maximumFiltering->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *maximumAdaptRate = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *disturbanceWindow = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *maximumTuning = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setVpeAccelerometerAdvancedTuning( + Vn100* vn100, + VnVector3 minimumFiltering, + VnVector3 maximumFiltering, + float maximumAdaptRate, + float disturbanceWindow, + float maximumTuning, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,39,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", minimumFiltering.c0, minimumFiltering.c1, minimumFiltering.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", maximumFiltering.c0, maximumFiltering.c1, maximumFiltering.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", maximumAdaptRate); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", disturbanceWindow); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", maximumTuning); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getVpeGyroBasicTuning( + Vn100* vn100, + VnVector3* varianceAngularWalk, + VnVector3* baseTuning, + VnVector3* adaptiveTuning) +{ + const char* cmdToSend = "$VNRRG,40"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + varianceAngularWalk->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + varianceAngularWalk->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + varianceAngularWalk->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setVpeGyroBasicTuning( + Vn100* vn100, + VnVector3 varianceAngularWalk, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,40,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", varianceAngularWalk.c0, varianceAngularWalk.c1, varianceAngularWalk.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", baseTuning.c0, baseTuning.c1, baseTuning.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", adaptiveTuning.c0, adaptiveTuning.c1, adaptiveTuning.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getFilterStatus( + Vn100* vn100, + unsigned short* solutionStatus, + float* yawUncertainty, + float* pitchUncertainty, + float* rollUncertainty, + float* gyroBiasUncertainty, + float* magUncertainty, + float* accelUncertainty) +{ + const char* cmdToSend = "$VNRRG,42"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *solutionStatus = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *yawUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *pitchUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *rollUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *gyroBiasUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelUncertainty = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getFilterStartupGyroBias( + Vn100* vn100, + VnVector3* gyroBias) +{ + const char* cmdToSend = "$VNRRG,43"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroBias->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroBias->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroBias->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setFilterStartupGyroBias( + Vn100* vn100, + VnVector3 gyroBias, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,43,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", gyroBias.c0, gyroBias.c1, gyroBias.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getMagnetometerCalibrationStatus( + Vn100* vn100, + unsigned char* lastBin, + unsigned short* numOfMeasurements, + float* avgResidual, + VnVector3* lastMeasurement, + unsigned char* bin0, + unsigned char* bin1, + unsigned char* bin2, + unsigned char* bin3, + unsigned char* bin4, + unsigned char* bin5, + unsigned char* bin6, + unsigned char* bin7) +{ + const char* cmdToSend = "$VNRRG,46"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *lastBin = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *numOfMeasurements = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *avgResidual = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lastMeasurement->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lastMeasurement->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lastMeasurement->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin0 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin1 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin2 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin3 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin4 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin5 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin6 = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *bin7 = (unsigned char) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getIndoorHeadingModeControl( + Vn100* vn100, + float* maxRateError, + float* reserved) +{ + const char* cmdToSend = "$VNRRG,48"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *maxRateError = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *reserved = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setIndoorHeadingModeControl( + Vn100* vn100, + float maxRateError, + float reserved, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,48,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", maxRateError); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", reserved); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getVelocityCompenstationControl( + Vn100* vn100, + uint8_t* mode, + float* velocityTuning, + float* rateTuning) +{ + const char* cmdToSend = "$VNRRG,51"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *mode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *velocityTuning = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *rateTuning = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setVelocityCompenstationControl( + Vn100* vn100, + uint8_t mode, + float velocityTuning, + float rateTuning, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,51,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", mode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", velocityTuning); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", rateTuning); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getVelocityCompenstationMeasurement( + Vn100* vn100, + VnVector3* velocity) +{ + const char* cmdToSend = "$VNRRG,50"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + velocity->c0 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + velocity->c1 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + velocity->c2 = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_setVelocityCompenstationMeasurement( + Vn100* vn100, + VnVector3 velocity, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,50,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", velocity.c0); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", velocity.c1); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f", velocity.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(&vn100->vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vn100_getYawPitchRollInertialCalibratedMeasurements( + Vn100* vn100, + VnYpr* attitude, + VnVector3* inertialMagnetic, + VnVector3* inertialAcceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,241"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialMagnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialMagnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialMagnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getRawVoltageMeasurements( + Vn100* vn100, + VnVector3* magnetometer, + VnVector3* accelerometer, + VnVector3* gyroscope, + float* temperature) +{ + const char* cmdToSend = "$VNRRG,251"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetometer->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetometer->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetometer->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerometer->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerometer->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + accelerometer->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscope->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscope->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscope->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *temperature = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getKalmanFilterStateVector( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* gyroscopeBias) +{ + const char* cmdToSend = "$VNRRG,253"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscopeBias->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscopeBias->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gyroscopeBias->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vn100_getKalmanFilterCovarianceMatrixDiagonal( + Vn100* vn100, + float* p00, + float* p11, + float* p22, + float* p33, + float* p44, + float* p55) +{ + const char* cmdToSend = "$VNRRG,254"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + errorCode = vndevice_transaction(&vn100->vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice_getResponseBuffer(&vn100->vndevice), delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p00 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p11 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p22 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p33 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p44 = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *p55 = (float) atof(result); + + return VNERR_NO_ERROR; +} + +int vn100_get_timeout( + Vn100* vn100) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_get_timeout( + &vn100->vndevice); +} + +VN_ERROR_CODE vn100_set_timeout( + Vn100* vn100, + int timeout) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_set_timeout( + &vn100->vndevice, + timeout); +} + +VN_ERROR_CODE vn100_getCurrentAsyncData( + Vn100* vn100, + VnDeviceCompositeData* curData) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getCurrentAsyncData( + &vn100->vndevice, + curData); +} + +VN_ERROR_CODE vn100_writeSettings( + Vn100* vn100, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_writeSettings( + &vn100->vndevice, + waitForResponse); +} + +VN_ERROR_CODE vn100_restoreFactorySettings( + Vn100* vn100, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_restoreFactorySettings( + &vn100->vndevice, + waitForResponse); +} + +VN_ERROR_CODE vn100_reset( + Vn100* vn100) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_reset( + &vn100->vndevice); +} + +VN_ERROR_CODE vn100_getUserTag( + Vn100* vn100, + char* userTagBuffer, + uint32_t userTagBufferLength) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getUserTag( + &vn100->vndevice, + userTagBuffer, + userTagBufferLength); +} + +VN_ERROR_CODE vn100_setUserTag( + Vn100* vn100, + char* userTagData, + uint32_t userTagDataLength, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setUserTag( + &vn100->vndevice, + userTagData, + userTagDataLength, + waitForResponse); +} + +VN_ERROR_CODE vn100_getModelNumber( + Vn100* vn100, + char* modelBuffer, + uint32_t modelBufferLength) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getModelNumber( + &vn100->vndevice, + modelBuffer, + modelBufferLength); +} + +VN_ERROR_CODE vn100_getHardwareRevision( + Vn100* vn100, + int32_t* hardwareRevision) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getHardwareRevision( + &vn100->vndevice, + hardwareRevision); +} + +VN_ERROR_CODE vn100_getSerialNumber( + Vn100* vn100, + char* serialNumberBuffer, + uint32_t serialNumberBufferLength) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getSerialNumber( + &vn100->vndevice, + serialNumberBuffer, + serialNumberBufferLength); +} + +VN_ERROR_CODE vn100_getFirmwareVersion( + Vn100* vn100, + char* firmwareVersionBuffer, + uint32_t firmwareVersionBufferLength) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getFirmwareVersion( + &vn100->vndevice, + firmwareVersionBuffer, + firmwareVersionBufferLength); +} + +VN_ERROR_CODE vn100_getSerialBaudRate( + Vn100* vn100, + uint32_t* serialBaudrate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getSerialBaudRate( + &vn100->vndevice, + serialBaudrate); +} + +VN_ERROR_CODE vn100_setSerialBaudRate( + Vn100* vn100, + uint32_t serialBaudrate, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setSerialBaudRate( + &vn100->vndevice, + serialBaudrate, + waitForResponse); +} + +VN_ERROR_CODE vn100_getAsynchronousDataOutputType( + Vn100* vn100, + uint32_t* asyncDataOutputType) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getAsynchronousDataOutputType( + &vn100->vndevice, + asyncDataOutputType); +} + +VN_ERROR_CODE vn100_setAsynchronousDataOutputType( + Vn100* vn100, + uint32_t asyncDataOutputType, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setAsynchronousDataOutputType( + &vn100->vndevice, + asyncDataOutputType, + waitForResponse); +} + +VN_ERROR_CODE vn100_getAsynchronousDataOutputFrequency( + Vn100* vn100, + uint32_t* asyncDataOutputFrequency) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getAsynchronousDataOutputFrequency( + &vn100->vndevice, + asyncDataOutputFrequency); +} + +VN_ERROR_CODE vn100_setAsynchronousDataOutputFrequency( + Vn100* vn100, + uint32_t asyncDataOutputFrequency, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setAsynchronousDataOutputFrequency( + &vn100->vndevice, + asyncDataOutputFrequency, + waitForResponse); +} + +VN_ERROR_CODE vn100_getYawPitchRoll( + Vn100* vn100, + VnYpr* attitude) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getYawPitchRoll( + &vn100->vndevice, + attitude); +} + +VN_ERROR_CODE vn100_getQuaternion( + Vn100* vn100, + VnQuaternion* attitude) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getQuaternion( + &vn100->vndevice, + attitude); +} + +VN_ERROR_CODE vn100_getYawPitchRollMagneticAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getYawPitchRollMagneticAccelerationAngularRate( + &vn100->vndevice, + attitude, + magnetic, + acceleration, + angularRate); +} + +VN_ERROR_CODE vn100_getQuaternionMagneticAccelerationAngularRate( + Vn100* vn100, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getQuaternionMagneticAccelerationAngularRate( + &vn100->vndevice, + attitude, + magnetic, + acceleration, + angularRate); +} + +VN_ERROR_CODE vn100_getMagnetic( + Vn100* vn100, + VnVector3* magnetic) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getMagnetic( + &vn100->vndevice, + magnetic); +} + +VN_ERROR_CODE vn100_getAcceleration( + Vn100* vn100, + VnVector3* acceleration) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getAcceleration( + &vn100->vndevice, + acceleration); +} + +VN_ERROR_CODE vn100_getAngularRate( + Vn100* vn100, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getAngularRate( + &vn100->vndevice, + angularRate); +} + +VN_ERROR_CODE vn100_getMagneticAccelerationAngularRate( + Vn100* vn100, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getMagneticAccelerationAngularRate( + &vn100->vndevice, + magnetic, + acceleration, + angularRate); +} + +VN_ERROR_CODE vn100_getYawPitchRollTrueBodyAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* bodyAcceleration, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getYawPitchRollTrueBodyAccelerationAngularRate( + &vn100->vndevice, + attitude, + bodyAcceleration, + angularRate); +} + +VN_ERROR_CODE vn100_getYawPitchRollTrueInertialAccelerationAngularRate( + Vn100* vn100, + VnYpr* attitude, + VnVector3* inertialAcceleration, + VnVector3* angularRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getYawPitchRollTrueInertialAccelerationAngularRate( + &vn100->vndevice, + attitude, + inertialAcceleration, + angularRate); +} + +VN_ERROR_CODE vn100_getVpeControl( + Vn100* vn100, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getVpeControl( + &vn100->vndevice, + enable, + headingMode, + filteringMode, + tuningMode); +} + +VN_ERROR_CODE vn100_setVpeControl( + Vn100* vn100, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setVpeControl( + &vn100->vndevice, + enable, + headingMode, + filteringMode, + tuningMode, + waitForResponse); +} + +VN_ERROR_CODE vn100_getVpeMagnetometerBasicTuning( + Vn100* vn100, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getVpeMagnetometerBasicTuning( + &vn100->vndevice, + baseTuning, + adaptiveTuning, + adaptiveFiltering); +} + +VN_ERROR_CODE vn100_setVpeMagnetometerBasicTuning( + Vn100* vn100, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setVpeMagnetometerBasicTuning( + &vn100->vndevice, + baseTuning, + adaptiveTuning, + adaptiveFiltering, + waitForResponse); +} + +VN_ERROR_CODE vn100_getVpeAccelerometerBasicTuning( + Vn100* vn100, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getVpeAccelerometerBasicTuning( + &vn100->vndevice, + baseTuning, + adaptiveTuning, + adaptiveFiltering); +} + +VN_ERROR_CODE vn100_setVpeAccelerometerBasicTuning( + Vn100* vn100, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setVpeAccelerometerBasicTuning( + &vn100->vndevice, + baseTuning, + adaptiveTuning, + adaptiveFiltering, + waitForResponse); +} + +VN_ERROR_CODE vn100_getImuMeasurements( + Vn100* vn100, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate, + float* temperature, + float* pressure) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getImuMeasurements( + &vn100->vndevice, + magnetic, + acceleration, + angularRate, + temperature, + pressure); +} + +VN_ERROR_CODE vn100_getReferenceFrameRotation( + Vn100* vn100, + VnMatrix3x3* c) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getReferenceFrameRotation( + &vn100->vndevice, + c); +} + +VN_ERROR_CODE vn100_setReferenceFrameRotation( + Vn100* vn100, + VnMatrix3x3 c, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setReferenceFrameRotation( + &vn100->vndevice, + c, + waitForResponse); +} + +VN_ERROR_CODE vn100_getSynchronizationControl( + Vn100* vn100, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getSynchronizationControl( + &vn100->vndevice, + syncInMode, + syncInEdge, + syncInSkipFactor, + syncOutMode, + syncOutPolarity, + syncOutSkipFactor, + syncOutPulseWidth); +} + +VN_ERROR_CODE vn100_setSynchronizationControl( + Vn100* vn100, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setSynchronizationControl( + &vn100->vndevice, + syncInMode, + syncInEdge, + syncInSkipFactor, + syncOutMode, + syncOutPolarity, + syncOutSkipFactor, + syncOutPulseWidth, + waitForResponse); +} + +VN_ERROR_CODE vn100_getSynchronizationStatus( + Vn100* vn100, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getSynchronizationStatus( + &vn100->vndevice, + syncInCount, + syncInTime, + syncOutCount); +} + +VN_ERROR_CODE vn100_setSynchronizationStatus( + Vn100* vn100, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setSynchronizationStatus( + &vn100->vndevice, + syncInCount, + syncInTime, + syncOutCount, + waitForResponse); +} + +VN_ERROR_CODE vn100_getDeltaThetaAndDeltaVelocity( + Vn100* vn100, + float* deltaTime, + VnVector3* deltaTheta, + VnVector3* deltaVelocity) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getDeltaThetaAndDeltaVelocity( + &vn100->vndevice, + deltaTime, + deltaTheta, + deltaVelocity); +} + +VN_ERROR_CODE vn100_getAccelerationCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getAccelerationCompensation( + &vn100->vndevice, + c, + b); +} + +VN_ERROR_CODE vn100_setAccelerationCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setAccelerationCompensation( + &vn100->vndevice, + c, + b, + waitForResponse); +} + +VN_ERROR_CODE vn100_getMagneticCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getMagneticCompensation( + &vn100->vndevice, + c, + b); +} + +VN_ERROR_CODE vn100_setMagneticCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setMagneticCompensation( + &vn100->vndevice, + c, + b, + waitForResponse); +} + +VN_ERROR_CODE vn100_getGyroCompensation( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getGyroCompensation( + &vn100->vndevice, + c, + b); +} + +VN_ERROR_CODE vn100_setGyroCompensation( + Vn100* vn100, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setGyroCompensation( + &vn100->vndevice, + c, + b, + waitForResponse); +} + +VN_ERROR_CODE vn100_getImuFilteringConfiguration( + Vn100* vn100, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getImuFilteringConfiguration( + &vn100->vndevice, + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode); +} + +VN_ERROR_CODE vn100_setImuFilteringConfiguration( + Vn100* vn100, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setImuFilteringConfiguration( + &vn100->vndevice, + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode, + waitForResponse); +} + +VN_ERROR_CODE vn100_getDeltaThetaAndDeltaVelocityConfiguration( + Vn100* vn100, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getDeltaThetaAndDeltaVelocityConfiguration( + &vn100->vndevice, + integrationFrame, + gyroCompensation, + accelCompensation); +} + +VN_ERROR_CODE vn100_setDeltaThetaAndDeltaVelocityConfiguration( + Vn100* vn100, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setDeltaThetaAndDeltaVelocityConfiguration( + &vn100->vndevice, + integrationFrame, + gyroCompensation, + accelCompensation, + waitForResponse); +} + +VN_ERROR_CODE vn100_getMagnetometerCalibrationControl( + Vn100* vn100, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getMagnetometerCalibrationControl( + &vn100->vndevice, + hsiMode, + hsiOutput, + convergeRate); +} + +VN_ERROR_CODE vn100_setMagnetometerCalibrationControl( + Vn100* vn100, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setMagnetometerCalibrationControl( + &vn100->vndevice, + hsiMode, + hsiOutput, + convergeRate, + waitForResponse); +} + +VN_ERROR_CODE vn100_getCalculatedMagnetometerCalibration( + Vn100* vn100, + VnMatrix3x3* c, + VnVector3* b) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getCalculatedMagnetometerCalibration( + &vn100->vndevice, + c, + b); +} + +VN_ERROR_CODE vn100_getMagneticGravityReferenceVectors( + Vn100* vn100, + VnVector3* magneticReference, + VnVector3* gravityReference) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getMagneticGravityReferenceVectors( + &vn100->vndevice, + magneticReference, + gravityReference); +} + +VN_ERROR_CODE vn100_setMagneticGravityReferenceVectors( + Vn100* vn100, + VnVector3 magneticReference, + VnVector3 gravityReference, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setMagneticGravityReferenceVectors( + &vn100->vndevice, + magneticReference, + gravityReference, + waitForResponse); +} + +VN_ERROR_CODE vn100_getCommunicationProtocolControl( + Vn100* vn100, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getCommunicationProtocolControl( + &vn100->vndevice, + serialCount, + serialStatus, + spiCount, + spiStatus, + serialChecksum, + spiChecksum, + errorMode); +} + +VN_ERROR_CODE vn100_setCommunicationProtocolControl( + Vn100* vn100, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setCommunicationProtocolControl( + &vn100->vndevice, + serialCount, + serialStatus, + spiCount, + spiStatus, + serialChecksum, + spiChecksum, + errorMode, + waitForResponse); +} + +VN_ERROR_CODE vn100_getReferenceVectorConfiguration( + Vn100* vn100, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint32_t* recalcThreshold, + float* year, + VnVector3* lla) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_getReferenceVectorConfiguration( + &vn100->vndevice, + useMagModel, + useGravityModel, + recalcThreshold, + year, + lla); +} + +VN_ERROR_CODE vn100_setReferenceVectorConfiguration( + Vn100* vn100, + uint8_t useMagModel, + uint8_t useGravityModel, + uint32_t recalcThreshold, + float year, + VnVector3 lla, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setReferenceVectorConfiguration( + &vn100->vndevice, + useMagModel, + useGravityModel, + recalcThreshold, + year, + lla, + waitForResponse); +} + +VN_ERROR_CODE vn100_pauseAsyncOutputs( + Vn100* vn100, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_pauseAsyncOutputs( + &vn100->vndevice, + waitForResponse); +} + +VN_ERROR_CODE vn100_resumeAsyncOutputs( + Vn100* vn100, + bool waitForResponse) +{ + if (!vn100->isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_resumeAsyncOutputs( + &vn100->vndevice, + waitForResponse); +} + +/** \endcond */ diff --git a/src/vectornav/src/vectornav/vncp_services.c b/src/vectornav/src/vectornav/vncp_services.c new file mode 100755 index 0000000..eeed805 --- /dev/null +++ b/src/vectornav/src/vectornav/vncp_services.c @@ -0,0 +1,424 @@ +/** + * \cond INCLUDE_PRIVATE + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This file supplies the cross-platform services when on a Linux machine. + */ +#include +#include +#include +#include +#include +#include +#include "vncp_services.h" +#include "vn_errorCodes.h" + +/* Private type declarations. ************************************************/ +typedef struct { + VN_THREAD_START_ROUTINE startRoutine; + void* routineData; +} VncpThreadStartData; + +typedef struct { + pthread_mutex_t mutex; + pthread_cond_t condition; + bool isTriggered; +} VncpConditionAndMutex; + +/* Private function declarations. ********************************************/ +void* vncp_thread_startRoutine(void* threadStartData); +VN_ERROR_CODE vncp_convertNativeToVnErrorCode(int nativeErrorCode); + +/** + * \brief Determines what the baudrate flag should be for the provide baudrate. + * + * \param[in] baudrate Desired baudrate. + * \return The appropriate baudrate flag to set in the termios.c_cflag field to + * get the desired baudrate. If the provided baudrate is invalid, the value B0 + * will be returned. + */ +tcflag_t vncp_determineBaudrateFlag(unsigned int baudrate); + +/* Private variables. */ +double _clockStart = -1.0; + +VN_ERROR_CODE vncp_thread_startNew(VN_HANDLE* newThreadHandle, VN_THREAD_START_ROUTINE startRoutine, void* routineData) +{ + int errorCode; + + VncpThreadStartData* data = (VncpThreadStartData*) malloc(sizeof(VncpThreadStartData)); + + data->startRoutine = startRoutine; + data->routineData = routineData; + + errorCode = pthread_create(&newThreadHandle->pThreadHandle, NULL, vncp_thread_startRoutine, data); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_open(VN_HANDLE* newComPortHandle, char const* portName, unsigned int baudrate) +{ + struct termios portSettings; + int portFd = -1; + tcflag_t baudrateFlag; + + portFd = open(portName, O_RDWR | O_NOCTTY); + if (portFd == -1) + return vncp_convertNativeToVnErrorCode(errno); + + /* clear struct for new port settings */ + memset(&portSettings, 0, sizeof(portSettings)); + + baudrateFlag = vncp_determineBaudrateFlag(baudrate); + if (baudrateFlag == B0) + return VNERR_UNKNOWN_ERROR; + + /* Set baudrate, 8n1, no modem control, enable receiving characters. */ + portSettings.c_cflag = baudrateFlag | CS8 | CLOCAL | CREAD; + + portSettings.c_iflag = IGNPAR; /* Ignore bytes with parity errors. */ + portSettings.c_oflag = 0; /* Enable raw data output. */ + + portSettings.c_cc[VTIME] = 0; /* Do not use inter-character timer. */ + portSettings.c_cc[VMIN] = 0; /* Block on reads until 0 character is received. */ + + /* Clear the COM port buffers. */ + if (tcflush(portFd, TCIFLUSH) != 0) + return vncp_convertNativeToVnErrorCode(errno); + + if (tcsetattr(portFd, TCSANOW, &portSettings) != 0) + return vncp_convertNativeToVnErrorCode(errno); + + newComPortHandle->comPortHandle = portFd; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_writeData(VN_HANDLE comPortHandle, char const* dataToWrite, unsigned int numOfBytesToWrite) +{ + int errorCode; + + errorCode = write(comPortHandle.comPortHandle, dataToWrite, numOfBytesToWrite); + + if (errorCode == -1) + return vncp_convertNativeToVnErrorCode(errno); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_readData(VN_HANDLE comPortHandle, char* readBuffer, unsigned int numOfBytesToRead, unsigned int* numOfBytesActuallyRead) +{ + int numOfBytesRead; + + *numOfBytesActuallyRead = 0; + + numOfBytesRead = read(comPortHandle.comPortHandle, readBuffer, numOfBytesToRead); + + if (numOfBytesRead == -1) + return vncp_convertNativeToVnErrorCode(errno); + + *numOfBytesActuallyRead = (unsigned int) numOfBytesRead; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_close(VN_HANDLE comPortHandle) +{ + if (close(comPortHandle.comPortHandle) == -1) + return vncp_convertNativeToVnErrorCode(errno); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_isOptimized( + char const* portName, + bool* isOptimized) +{ + /* Currently Linux USB COM ports don't need any optimization. */ + *isOptimized = true; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_comPort_optimize( + char const* portName) +{ + /* Nothing necessary to do on Linux machines. */ + return VNERR_NO_ERROR; +} + +tcflag_t vncp_determineBaudrateFlag(unsigned int baudrate) +{ + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + #if !defined(__QNXNTO__) /* QNX does not have higher baudrates defined. */ + case 230400: return B230400; + case 460800: return B460800; + case 921600: return B921600; + #endif + default: return B0; + } +} + +VN_ERROR_CODE vncp_event_create(VN_HANDLE* newEventHandle) +{ + int errorCode; + VncpConditionAndMutex* cm; + + cm = (VncpConditionAndMutex*) malloc(sizeof(VncpConditionAndMutex)); + + errorCode = pthread_mutex_init(&cm->mutex, NULL); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + errorCode = pthread_cond_init(&cm->condition, NULL); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + cm->isTriggered = false; + + newEventHandle->conditionAndMutexStruct = cm; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_event_waitFor(VN_HANDLE eventHandle, int timeout) +{ + int errorCode, loopErrorCode; + VncpConditionAndMutex* cm; + struct timespec delta; + struct timespec abstime; + + // Compute our timeout. + if (timeout != -1) { + + clock_gettime(CLOCK_REALTIME, &abstime); + + int nano = abstime.tv_nsec + (timeout % 1000) * 1000000; + + delta.tv_nsec = nano % 1000000000; + delta.tv_sec = abstime.tv_sec + timeout / 1000 + nano / 1000000000; + } + + cm = (VncpConditionAndMutex*) eventHandle.conditionAndMutexStruct; + + errorCode = pthread_mutex_lock(&cm->mutex); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + while (!cm->isTriggered) { + + if (timeout == -1) { + loopErrorCode = pthread_cond_wait(&cm->condition, &cm->mutex); + + if (loopErrorCode != 0) + return vncp_convertNativeToVnErrorCode(loopErrorCode); + } + else { + loopErrorCode = pthread_cond_timedwait(&cm->condition, &cm->mutex, &delta); + + if (loopErrorCode == ETIMEDOUT) { + cm->isTriggered = false; + errorCode = pthread_mutex_unlock(&cm->mutex); + + return VNERR_TIMEOUT; + } + } + } + + cm->isTriggered = false; + + errorCode = pthread_mutex_unlock(&cm->mutex); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_event_signal(VN_HANDLE eventHandle) +{ + int errorCode; + VncpConditionAndMutex* cm; + + cm = (VncpConditionAndMutex*) eventHandle.conditionAndMutexStruct; + + errorCode = pthread_mutex_lock(&cm->mutex); + + cm->isTriggered = true; + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + errorCode = pthread_cond_signal(&cm->condition); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + errorCode = pthread_mutex_unlock(&cm->mutex); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_criticalSection_initialize(VN_CRITICAL_SECTION* criticalSection) +{ + int errorCode; + + errorCode = pthread_mutex_init(criticalSection, NULL); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_criticalSection_enter(VN_CRITICAL_SECTION* criticalSection) +{ + int errorCode; + + errorCode = pthread_mutex_lock(criticalSection); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_criticalSection_leave(VN_CRITICAL_SECTION* criticalSection) +{ + int errorCode; + + errorCode = pthread_mutex_unlock(criticalSection); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vncp_criticalSection_dispose(VN_CRITICAL_SECTION* criticalSection) +{ + int errorCode; + + errorCode = pthread_mutex_destroy(criticalSection); + + if (errorCode != 0) + return vncp_convertNativeToVnErrorCode(errorCode); + + return VNERR_NO_ERROR; +} + +void* vncp_thread_startRoutine(void* threadStartData) +{ + VncpThreadStartData* data; + + data = (VncpThreadStartData*) threadStartData; + + /* Call the user's thread routine. */ + data->startRoutine(data->routineData); + + return 0; +} + +VN_ERROR_CODE vncp_convertNativeToVnErrorCode(int nativeErrorCode) +{ + switch (nativeErrorCode) { + + case ENOENT: + return VNERR_FILE_NOT_FOUND; + + case EACCES: + return VNERR_PERMISSION_DENIED; + + default: + return VNERR_UNKNOWN_ERROR; + } +} + +VN_ERROR_CODE vncp_sleepInMs(unsigned int numOfMillisecondsToSleep) +{ + usleep(numOfMillisecondsToSleep * 1000); + + return VNERR_NO_ERROR; +} + +void vncp_startMsTimer() +{ + struct timespec time; + int error; + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error != 0) { + + _clockStart = -1.0; + + return; + } + + _clockStart = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0); +} + +double vncp_stopMsTimer() +{ + struct timespec time; + int error; + double result; + + if (_clockStart < 0) + return -1.0; + + error = clock_gettime(CLOCK_MONOTONIC, &time); + + if (error != 0) { + + _clockStart = -1.0; + + return -1.0; + } + + result = (time.tv_sec * 1000.0) + (time.tv_nsec / 1000000.0) - _clockStart; + + _clockStart = -1.0; + + return result; +} + +/** \endcond */ diff --git a/src/vectornav/src/vectornav/vndevice.c b/src/vectornav/src/vectornav/vndevice.c new file mode 100755 index 0000000..b82aa13 --- /dev/null +++ b/src/vectornav/src/vectornav/vndevice.c @@ -0,0 +1,5581 @@ +/** + * \cond INCLUDE_PRIVATE + * \file + * + * \section LICENSE + * The MIT License (MIT) + * + * Copyright (c) 2014 VectorNav Technologies, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \section DESCRIPTION + * This file implements functionality common between VectorNav devices. + */ +#include +#include +#include +#include "vndevice.h" + +/* Defines and constants. ****************************************************/ + +/*#define COMMAND_HEADER_SIZE 5*/ +#define ASCII_RECEIVE_BUFFER_SIZE 256 +#define BINARY_RECEIVE_BUFFER_SIZE 256 +#define READ_BUFFER_SIZE 256 +#define NUMBER_OF_MILLISECONDS_TO_SLEEP_AFTER_RECEIVING_NO_BYTES_ON_COM_PORT_READ 5 +#define DEFAULT_TIMEOUT_IN_MS 5000 +#define ASCII_START_CHAR '$' +#define ASCII_FIRST_END_CHAR '\r' +#define ASCII_SECOND_END_CHAR '\n' +#define BINARY_START_CHAR ((char) 0xFA) + +/* Collection of group lengths for binary packets. This array is copied from the + VN-200 user manual. */ +const unsigned char BinaryPacketGroupLengths[6][16] = { + {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8, 0}, /* Group 1 */ + {8, 8, 8, 2, 8, 8, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0}, /* Group 2 */ + {2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0, 0}, /* Group 3 */ + {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 32, 0, 0, 0}, /* Group 4 */ + {2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 0, 0, 0, 0}, /* Group 5 */ + {2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0, 0}, /* Group 6 */ +}; + +/* Private function definitions. *********************************************/ + +/** + * \brief Indicates whether the comPortServiceThread should be checking + * incoming data packets for matches to a command sent out. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \param[out] responseMatchBuffer + * Buffer where the match string will be placed if response checking is current + * enabled. The size of this buffer should be VN_RESPONSE_MATCH_SIZE + 1. The + * returned string will be null-terminated. + * + * \return true if response checking should be performed; false if no + * checking should be performed. + */ +bool vndevice_shouldCheckForResponse_threadSafe( + VnDevice* vndevice, + char* responseMatchBuffer); + +/** + * \brief Disable response checking by the comPortServiceThread. + * + * \param[in] vndevice Pointer to the VnDevice control object. + */ +void vndevice_disableResponseChecking_threadSafe( + VnDevice* vndevice); + +/** + * \brief Reads data from the connected COM port in a thread-safe manner. + * + * Reads data from the connected COM port in a thread-safe manner to avoid + * conflicts between the comPortServiceThread and the user thread. Use only the + * functions vndevice_writeData_threadSafe and vndevice_readData_threadSafe to ensure + * communcation over the COM port is thread-safe. + */ +int vndevice_readData_threadSafe( + VnDevice* vndevice, + char* dataBuffer, + unsigned int numOfBytesToRead, + unsigned int* numOfBytesActuallyRead); + +/** + * \brief Sends out data over the connected COM port in a thread-safe manner. + * + * Sends out data over the connected COM port in a thread-safe manner to avoid + * conflicts between the comPortServiceThread and the user thread. Use only the + * functions vn100_writeData_threadSafe and vn100_readData_threadSafe to ensure + * communcation over the COM port is thread-safe. + */ +int vndevice_writeData_threadSafe( + VnDevice* vndevice, + const char* dataToSend, + unsigned int dataLength); + +/** + * \brief Enabled response checking by the comPortServiceThread. + * + * \param[in] vndevice Pointer to the VnDevice control object. + * + * \param[in] responseMatch + * Null-terminated string with a maximum length of VN_RESPONSE_MATCH_SIZE which + * will be used by the comPortServiceThread to detect a received data packet + * is an appropriate match to a command sent to the VectorNav device. + */ +void vndevice_enableResponseChecking_threadSafe( + VnDevice* vndevice, + const char* responseMatch); + +void vndevice_processAsyncData( + VnDevice* vndevice, + char* buffer); + +void* vndevice_communicationHandler( + void* vndevice); + +void vndevice_processReceivedPacket( + VnDevice* vndevice, + char* buffer); + +unsigned char vndevice_numberOfSetBits( + unsigned char toCount); + +int vndevice_computeLengthOfExpectedBinaryPayload( + char* ptrToPacketStart); + +void vndevice_processReceivedBinaryPacket( + VnDevice* vndevice, + char* buffer); + +uint16_t vndevice_processGroup1Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +uint16_t vndevice_processGroup2Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +uint16_t vndevice_processGroup3Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +uint16_t vndevice_processGroup4Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +uint16_t vndevice_processGroup5Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +uint16_t vndevice_processGroup6Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data); + +/** + * \brief Converts a sensor error code into the library's system error code. + * + * \param[in] sensorError The sensor error code. + * + * \return The sensor error code converted to the library's system error code. + */ +VN_ERROR_CODE vndevice_convertSensorErrorToSystemError( + uint8_t sensorError); + +/* Function implementations. *************************************************/ + +VN_ERROR_CODE vndevice_convertSensorErrorToSystemError( + uint8_t sensorError) +{ + return sensorError + VNERR_PERMISSION_DENIED; +} + +int vndevice_computeLengthOfBinaryGroupPayload( + unsigned char groupIndex, + uint16_t groupField) +{ + unsigned char i; + int runningLength = 0; + + for (i = 0; i < sizeof (uint16_t) * 8; i++) { + + if ((groupField >> i) & 1) { + runningLength += BinaryPacketGroupLengths[groupIndex][i]; + } + } + + return runningLength; +} + +VN_ERROR_CODE vndevice_initializeVnDevice( + VnDevice* vndevice, + const char* portName, + int baudrate, + void* deviceMask) +{ + VN_ERROR_CODE errorCode; + + vndevice->asyncDataListener = NULL; + vndevice->errorCodeListener = NULL; + vndevice->sensorError = 0; + vndevice->continueServicingComPort = true; + vndevice->deviceMask = deviceMask; + vndevice->checkForResponse = false; + vndevice->timeout = DEFAULT_TIMEOUT_IN_MS; + + memset(&vndevice->lastestAsyncData, 0, sizeof(VnDeviceCompositeData)); + + errorCode = vncp_comPort_open(&vndevice->comPortHandle, portName, baudrate); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_criticalSection_initialize(&vndevice->critSecForComPort); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_criticalSection_initialize(&vndevice->critSecForResponseMatchAccess); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_event_create(&vndevice->waitForThreadToStopServicingComPortEvent); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_event_create(&vndevice->waitForCommandResponseEvent); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_event_create(&vndevice->waitForThreadToStartServicingComPortEvent); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + errorCode = vncp_criticalSection_initialize(&vndevice->critSecForLatestAsyncDataAccess); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_deinitializeVnDevice( + VnDevice* vndevice) +{ + vndevice->continueServicingComPort = false; + + vncp_event_waitFor(vndevice->waitForThreadToStopServicingComPortEvent, -1); + + vncp_comPort_close(vndevice->comPortHandle); + + vncp_criticalSection_dispose(&vndevice->critSecForComPort); + + vncp_criticalSection_dispose(&vndevice->critSecForResponseMatchAccess); + + vncp_criticalSection_dispose(&vndevice->critSecForLatestAsyncDataAccess); + + return VNERR_NO_ERROR; +} + +char* vndevice_getResponseBuffer( + VnDevice* vndevice) +{ + return vndevice->cmdResponseBuffer; +} + +VN_ERROR_CODE vndevice_set_timeout( + VnDevice* vndevice, + int timeout) +{ + if (timeout < -1) + return VNERR_INVALID_VALUE; + + vndevice->timeout = timeout; + + return VNERR_NO_ERROR; +} + +int vndevice_get_timeout( + VnDevice* vndevice) +{ + return vndevice->timeout; +} + +VN_ERROR_CODE vndevice_getCurrentAsyncData( + VnDevice* vndevice, + VnDeviceCompositeData* curData) +{ + vncp_criticalSection_enter(&vndevice->critSecForLatestAsyncDataAccess); + + memcpy(curData, &vndevice->lastestAsyncData, sizeof(VnDeviceCompositeData)); + + vncp_criticalSection_leave(&vndevice->critSecForLatestAsyncDataAccess); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_registerAsyncDataReceivedListener( + VnDevice* vndevice, + VnDeviceNewAsyncDataReceivedListener listener) +{ + /* We can only handle one listener for now. */ + if (vndevice->asyncDataListener != NULL) + return VNERR_UNKNOWN_ERROR; + + vndevice->asyncDataListener = listener; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_unregisterAsyncDataReceivedListener( + VnDevice* vndevice, + VnDeviceNewAsyncDataReceivedListener listener) +{ + if (vndevice->asyncDataListener == NULL) + return VNERR_UNKNOWN_ERROR; + + if (vndevice->asyncDataListener != listener) + return VNERR_UNKNOWN_ERROR; + + vndevice->asyncDataListener = NULL; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_registerErrorCodeReceivedListener( + VnDevice* vndevice, + VnDeviceErrorCodeReceivedListener listener) +{ + /* We can only handle one listener for now. */ + if (vndevice->errorCodeListener != NULL) + return VNERR_UNKNOWN_ERROR; + + vndevice->errorCodeListener = listener; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_unregisterErrorCodeReceivedListener( + VnDevice* vndevice, + VnDeviceErrorCodeReceivedListener listener) +{ + if (vndevice->errorCodeListener == NULL) + return VNERR_UNKNOWN_ERROR; + + if (vndevice->errorCodeListener != listener) + return VNERR_UNKNOWN_ERROR; + + vndevice->errorCodeListener = NULL; + + return VNERR_NO_ERROR; +} + +void vndevice_processReceivedBinaryPacket( + VnDevice* vndevice, + char* buffer) +{ + VnDeviceCompositeData data; + uint8_t groups; + uint8_t numberOfTotalGroupFieldBytes; + char* curDataPointer; + char* curGroupFieldPointer; + uint16_t numberOfDataBytesProcessed; + + memset(&data, 0, sizeof(VnDeviceCompositeData)); + + groups = *((uint8_t*) (buffer + 1)); + + numberOfTotalGroupFieldBytes = vndevice_numberOfSetBits(groups) * 2; + + curGroupFieldPointer = buffer + 1 + 1; + + curDataPointer = buffer + 1 + 1 + numberOfTotalGroupFieldBytes; + + if (groups & 0x01) { + + numberOfDataBytesProcessed = vndevice_processGroup1Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + if (groups & 0x02) { + + numberOfDataBytesProcessed = vndevice_processGroup2Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + if (groups & 0x04) { + + numberOfDataBytesProcessed = vndevice_processGroup3Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + if (groups & 0x08) { + + numberOfDataBytesProcessed = vndevice_processGroup4Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + if (groups & 0x10) { + + numberOfDataBytesProcessed = vndevice_processGroup5Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + if (groups & 0x20) { + + numberOfDataBytesProcessed = vndevice_processGroup6Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); + + curDataPointer += numberOfDataBytesProcessed; + + curGroupFieldPointer += sizeof (uint16_t); + } + + /* We had an async data packet and need to move it to vndevice->lastestAsyncData. */ + vncp_criticalSection_enter(&vndevice->critSecForLatestAsyncDataAccess); + memcpy(&vndevice->lastestAsyncData, &data, sizeof(VnDeviceCompositeData)); + vncp_criticalSection_leave(&vndevice->critSecForLatestAsyncDataAccess); + + if (vndevice->asyncDataListener != NULL) + vndevice->asyncDataListener(vndevice, &vndevice->lastestAsyncData); +} + +uint16_t vndevice_processGroup1Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + data->timeStartup = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0002) { + + data->timeGps = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0004) { + + data->timeSyncIn = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0008) { + + data->ypr.yaw = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->ypr.pitch = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->ypr.roll = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0010) { + + data->quaternion.x = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.y = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.z = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.w = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0020) { + + data->angularRate.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRate.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRate.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0040) { + + data->latitudeLongitudeAltitude.c0 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->latitudeLongitudeAltitude.c1 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->latitudeLongitudeAltitude.c2 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + } + + if (groupField & 0x0080) { + + data->velocity.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velocity.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velocity.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0100) { + + data->acceleration.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->acceleration.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->acceleration.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0200) { + + data->angularRateUncompensated.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRateUncompensated.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRateUncompensated.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelerationUncompensated.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelerationUncompensated.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelerationUncompensated.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0400) { + + data->magnetic.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magnetic.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magnetic.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->temperature = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->pressure = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0800) { + + data->deltaTime = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaVelocity.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaVelocity.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaVelocity.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x1000) { + + data->insStatus = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x2000) { + + data->syncInCnt = *((uint32_t*) groupDataPtr); + + groupDataPtr += sizeof (uint32_t); + } + + if (groupField & 0x4000) { + + data->timeGpsPps = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + +uint16_t vndevice_processGroup2Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + data->timeStartup = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0002) { + + data->timeGps = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0004) { + + data->gpsTowNs = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0008) { + + data->gpsWeek = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0010) { + + data->timeSyncIn = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0020) { + + data->timeGpsPps = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0040) { + + data->timeUtc.year = *((int8_t*) groupDataPtr); + + groupDataPtr += sizeof (int8_t); + + data->timeUtc.month = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.day = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.hour = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.min = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.sec = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.ms = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0080) { + + data->syncInCnt = *((uint32_t*) groupDataPtr); + + groupDataPtr += sizeof (uint32_t); + } + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + +uint16_t vndevice_processGroup3Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + /* ImuStatus is currently not used. */ + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0002) { + + data->magneticUncompensated.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magneticUncompensated.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magneticUncompensated.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0004) { + + data->accelerationUncompensated.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelerationUncompensated.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelerationUncompensated.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0008) { + + data->angularRateUncompensated.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRateUncompensated.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRateUncompensated.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0010) { + + data->temperature = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0020) { + + data->pressure = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0040) { + + data->deltaTime = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaTheta.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0080) { + + data->deltaVelocity.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaVelocity.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->deltaVelocity.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0100) { + + data->magnetic.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magnetic.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magnetic.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0200) { + + data->acceleration.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->acceleration.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->acceleration.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0400) { + + data->angularRate.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRate.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->angularRate.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0800) { + + data->sensSat = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + +uint16_t vndevice_processGroup4Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + data->timeUtc.year = *((int8_t*) groupDataPtr); + + groupDataPtr += sizeof (int8_t); + + data->timeUtc.month = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.day = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.hour = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.min = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.sec = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + + data->timeUtc.ms = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0002) { + + data->gpsTowNs = *((uint64_t*) groupDataPtr); + + groupDataPtr += sizeof (uint64_t); + } + + if (groupField & 0x0004) { + + data->gpsWeek = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0008) { + + data->numSats = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + } + + if (groupField & 0x0010) { + + data->gpsFix = *((uint8_t*) groupDataPtr); + + groupDataPtr += sizeof (uint8_t); + } + + if (groupField & 0x0020) { + + data->gpsPosLla.c0 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->gpsPosLla.c1 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->gpsPosLla.c2 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + } + + if (groupField & 0x0040) { + + data->gpsPosEcef.c0 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->gpsPosEcef.c1 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->gpsPosEcef.c2 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + } + + if (groupField & 0x0080) { + + data->gpsVelocity.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsVelocity.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsVelocity.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0100) { + + data->gpsVelEcef.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsVelEcef.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsVelEcef.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0200) { + + data->gpsPosU.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsPosU.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->gpsPosU.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0400) { + + data->gpsVelU = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0800) { + + data->timeU = *((uint32_t*) groupDataPtr); + + groupDataPtr += sizeof (uint32_t); + } + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + +uint16_t vndevice_processGroup5Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + data->vpeStatus = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0002) { + + data->ypr.yaw = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->ypr.pitch = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->ypr.roll = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0004) { + + data->quaternion.x = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.y = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.z = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->quaternion.w = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0008) { + + data->dcm.c00 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c10 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c20 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c01 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c11 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c21 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c02 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c12 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->dcm.c22 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0010) { + + data->magNed.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magNed.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magNed.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0020) { + + data->accelNed.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelNed.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelNed.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0040) { + + data->linearAccelBody.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelBody.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelBody.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0080) { + + data->linearAccelNed.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelNed.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelNed.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0100) { + + data->yprU.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->yprU.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->yprU.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + +uint16_t vndevice_processGroup6Data( + uint16_t groupField, + char* groupDataPtr, + VnDeviceCompositeData* data) +{ + char* originalGroupDataPtr = groupDataPtr; + + if (groupField & 0x0001) { + + data->insStatus = *((uint16_t*) groupDataPtr); + + groupDataPtr += sizeof (uint16_t); + } + + if (groupField & 0x0002) { + + data->latitudeLongitudeAltitude.c0 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->latitudeLongitudeAltitude.c1 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->latitudeLongitudeAltitude.c2 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + } + + if (groupField & 0x0004) { + + data->posEcef.c0 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->posEcef.c1 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + + data->posEcef.c2 = *((double*) groupDataPtr); + + groupDataPtr += sizeof (double); + } + + if (groupField & 0x0008) { + + data->velBody.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velBody.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velBody.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0010) { + + data->velNed.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velNed.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velNed.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0020) { + + data->velEcef.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velEcef.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->velEcef.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0040) { + + data->magEcef.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magEcef.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->magEcef.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0080) { + + data->accelEcef.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelEcef.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->accelEcef.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0100) { + + data->linearAccelEcef.c0 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelEcef.c1 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + + data->linearAccelEcef.c2 = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0200) { + + data->posU = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + if (groupField & 0x0400) { + + data->velU = *((float*) groupDataPtr); + + groupDataPtr += sizeof (float); + } + + + return (uint16_t) (groupDataPtr - originalGroupDataPtr); +} + + +unsigned char vndevice_numberOfSetBits( + unsigned char toCount) +{ + unsigned char i; + unsigned char numberOfSetBits = 0; + + for (i = 0; i < sizeof(unsigned char) * 8; i++) + { + if (toCount & 1) + numberOfSetBits++; + + toCount = toCount >> 1; + } + + return numberOfSetBits; +} + +VN_ERROR_CODE vndevice_startHandlingCommunication( + VnDevice* vndevice) +{ + VN_ERROR_CODE errorCode; + + errorCode = vncp_thread_startNew(&vndevice->comPortServiceThreadHandle, &vndevice_communicationHandler, vndevice); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_waitForThreadToStartHandlingCommunicationPort( + VnDevice* vndevice) +{ + VN_ERROR_CODE errorCode; + + errorCode = vncp_event_waitFor(vndevice->waitForThreadToStartServicingComPortEvent, -1); + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return VNERR_NO_ERROR; +} + +int vndevice_writeData_threadSafe( + VnDevice* vndevice, + const char* dataToSend, + unsigned int dataLength) +{ + int errorCode; + + vncp_criticalSection_enter(&vndevice->critSecForComPort); + errorCode = vncp_comPort_writeData(vndevice->comPortHandle, dataToSend, dataLength); + vncp_criticalSection_leave(&vndevice->critSecForComPort); + + return errorCode; +} + +int vndevice_readData_threadSafe( + VnDevice* vndevice, + char* dataBuffer, + unsigned int numOfBytesToRead, + unsigned int* numOfBytesActuallyRead) +{ + int errorCode; + + vncp_criticalSection_enter(&vndevice->critSecForComPort); + errorCode = vncp_comPort_readData(vndevice->comPortHandle, dataBuffer, numOfBytesToRead, numOfBytesActuallyRead); + vncp_criticalSection_leave(&vndevice->critSecForComPort); + + return errorCode; +} + +void vndevice_enableResponseChecking_threadSafe( + VnDevice* vndevice, + const char* responseMatch) +{ + vncp_criticalSection_enter(&vndevice->critSecForResponseMatchAccess); + + vndevice->checkForResponse = true; + strcpy(vndevice->cmdResponseMatchBuffer, responseMatch); + + vncp_criticalSection_leave(&vndevice->critSecForResponseMatchAccess); +} + +void vndevice_checksum_computeAndReturnAsHex( + const char* cmdToCheck, + char* checksum) +{ + unsigned char cs; + char tempChecksumHolder[3]; + + cs = vndevice_checksum_computeCrc8FromCommand(cmdToCheck); + + /* We cannot sprintf into the parameter checksum because sprintf always + appends a null at the end. */ + sprintf(tempChecksumHolder, "%02X", cs); + + checksum[0] = tempChecksumHolder[0]; + checksum[1] = tempChecksumHolder[1]; +} + +uint8_t vndevice_checksum_computeCrc8FromCommand( + const char* cmdToCheck) +{ + int i; + unsigned char xorVal = 0; + int cmdLength; + + cmdLength = strlen(cmdToCheck); + + for (i = 0; i < cmdLength; i++) + xorVal ^= (uint8_t) cmdToCheck[i]; + + return xorVal; +} + +uint16_t vndevice_checksum_computeCrc16( + const char data[], + uint32_t length) +{ + uint32_t i; + uint16_t crc = 0; + + for (i = 0; i < length; i++) { + + crc = (uint16_t) ((crc >> 8) | (crc << 8)); + + crc ^= (uint8_t) data[i]; + crc ^= (uint16_t) ((uint8_t)(crc & 0xFF) >> 4); + crc ^= (uint16_t) ((crc << 8) << 4); + crc ^= (uint16_t) (((crc & 0xFF) << 4) << 1); + } + + return crc; +} + +uint16_t vndevice_checksum_computeCrc16FromCommand( + const char* cmdToCheck) +{ + int cmdLength; + + cmdLength = strlen(cmdToCheck); + + return vndevice_checksum_computeCrc16( + cmdToCheck, + cmdLength); +} + +VN_ERROR_CODE vndevice_writeOutCommand( + VnDevice* vndevice, + const char* cmdToSend) +{ + char packetTail[] = "*FF\r\n"; + + /* We add one to the cmdToSend pointer to skip over the '$' at the beginning. */ + /* We add one to the packetTail pointer so the "FF" string is overwritten with the checksum. */ + vndevice_checksum_computeAndReturnAsHex(cmdToSend + 1, packetTail + 1); + + vndevice_writeData_threadSafe(vndevice, cmdToSend, strlen(cmdToSend)); + vndevice_writeData_threadSafe(vndevice, packetTail, strlen(packetTail)); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_transaction( + VnDevice* vndevice, + const char* cmdToSend, + const char* responseMatch) +{ + char packetTail[] = "*FF\r\n"; + VN_ERROR_CODE errorCode; + + vndevice->sensorError = 0; + + /* We add one to the cmdToSend pointer to skip over the '$' at the beginning. */ + /* We add one to the packetTail pointer so the "FF" string is overwritten with the checksum. */ + vndevice_checksum_computeAndReturnAsHex(cmdToSend + 1, packetTail + 1); + + vndevice_enableResponseChecking_threadSafe(vndevice, responseMatch); + + vndevice_writeData_threadSafe(vndevice, cmdToSend, strlen(cmdToSend)); + vndevice_writeData_threadSafe(vndevice, packetTail, strlen(packetTail)); + + errorCode = vncp_event_waitFor(vndevice->waitForCommandResponseEvent, vndevice->timeout); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return vndevice->sensorError; +} + +void* vndevice_communicationHandler( + void* vndeviceObj) +{ + VnDevice* vndevice; + unsigned int asciiBufferIndex = 0; + char asciiReceiveBuffer[ASCII_RECEIVE_BUFFER_SIZE]; + bool possibleStartOfAsciiPacketFound = false; + bool possibleFirstEndDelimiterOfAsciiPacketFound = false; + unsigned int binaryBufferIndex = 0; + char binaryReceiveBuffer[BINARY_RECEIVE_BUFFER_SIZE]; + char readBuffer[READ_BUFFER_SIZE]; + bool possibleStartOfBinaryPacketFound = false; + bool binaryGroupsFieldFound = false; + char expectedNumberOfBinaryGroupFieldBytes = 0; + char numberOfBinaryGroupFieldBytesProcessed = 0; + unsigned int expectedBinaryPacketLength = 0; + + unsigned char binaryGroups = 0; + unsigned int numOfBytesRead = 0; + + vndevice = (VnDevice*) vndeviceObj; + + vncp_event_signal(vndevice->waitForThreadToStartServicingComPortEvent); + + while (vndevice->continueServicingComPort) { + + unsigned int curResponsePos = 0; + + vndevice_readData_threadSafe(vndevice, readBuffer, READ_BUFFER_SIZE, &numOfBytesRead); + + if (numOfBytesRead == 0) { + /* There was no data. Sleep for a short amount of time before continuing. */ + vncp_sleepInMs(NUMBER_OF_MILLISECONDS_TO_SLEEP_AFTER_RECEIVING_NO_BYTES_ON_COM_PORT_READ); + continue; + } + + for ( ; curResponsePos < numOfBytesRead; curResponsePos++) { + + char curChar = readBuffer[curResponsePos]; + + /* Make sure we are not overrunning our ASCII buffer. */ + if (asciiBufferIndex == ASCII_RECEIVE_BUFFER_SIZE) { + + /* We are about to overflow our buffer. Reset the index and + start looking for another ASCII packet. */ + asciiBufferIndex = 0; + possibleStartOfAsciiPacketFound = false; + } + + /* See if we have possible found the start of a new ASCII packet. + Note that we only check if we have found the character '$' and + do not also check if possibleStartOfAsciiPacketFound == false. + This is because if we are also in the condition + possibleStartOfAsciiPacketFound == true and we find another + '$' character, which should not be in any ASCII packet except at + the beginning of the packet, we will start over looking for an + ASCII packet in. */ + if (readBuffer[curResponsePos] == ASCII_START_CHAR) { + + /* Start of command has been found. */ + possibleStartOfAsciiPacketFound = true; + asciiBufferIndex = 0; + asciiReceiveBuffer[asciiBufferIndex] = curChar; + asciiBufferIndex++; + } + else if (possibleStartOfAsciiPacketFound) { + + /* We are in the process of handling receipt of an ASCII packet. */ + + if (!possibleFirstEndDelimiterOfAsciiPacketFound && readBuffer[curResponsePos] == ASCII_FIRST_END_CHAR) { + + /* We have found the first end delimiter character. */ + possibleFirstEndDelimiterOfAsciiPacketFound = true; + asciiReceiveBuffer[asciiBufferIndex] = curChar; + asciiBufferIndex++; + + } + + else if (possibleFirstEndDelimiterOfAsciiPacketFound && readBuffer[curResponsePos] == ASCII_SECOND_END_CHAR) { + + /* We have found a complete ASCII packet now. */ + asciiReceiveBuffer[asciiBufferIndex] = curChar; + asciiBufferIndex++; + possibleFirstEndDelimiterOfAsciiPacketFound = false; + possibleStartOfAsciiPacketFound = false; + + /* Make sure we have enough room to append null termination. */ + if (asciiBufferIndex == ASCII_RECEIVE_BUFFER_SIZE) { + + /* We are about to overflow our buffer so we cannot continue processing. */ + + } + else { + + asciiReceiveBuffer[asciiBufferIndex] = '\0'; + asciiBufferIndex++; + + /* Found a packet, now dispatch it. */ + vndevice_processReceivedPacket(vndevice, asciiReceiveBuffer); + } + + asciiBufferIndex = 0; + } + + else { + + /* The previous character we handled might have been an '/r'. + Let's clear out the flag that we might have found the end + sequence of a packet. */ + possibleFirstEndDelimiterOfAsciiPacketFound = false; + + /* Copy over the found char. */ + asciiReceiveBuffer[asciiBufferIndex] = curChar; + asciiBufferIndex++; + } + } + + /* Make sure we are not overrunning our BINARY buffer. */ + if (binaryBufferIndex == BINARY_RECEIVE_BUFFER_SIZE) { + + /* We are about to overflow our buffer. Reset the index and + start looking for another BINARY packet. */ + binaryBufferIndex = 0; + possibleStartOfBinaryPacketFound = false; + binaryGroupsFieldFound = false; + binaryGroups = 0; + } + + if (!possibleStartOfBinaryPacketFound && curChar == BINARY_START_CHAR) { + + /* We have possibly found the start of a binary packet. */ + possibleStartOfBinaryPacketFound = true; + binaryReceiveBuffer[binaryBufferIndex] = curChar; + binaryBufferIndex++; + } + + else if (possibleStartOfBinaryPacketFound) { + + if (!binaryGroupsFieldFound) { + + /* We now have a possible binary packet groups field. */ + binaryGroups = curChar; + binaryGroupsFieldFound = true; + + binaryReceiveBuffer[binaryBufferIndex] = curChar; + binaryBufferIndex++; + + expectedNumberOfBinaryGroupFieldBytes = vndevice_numberOfSetBits(binaryGroups) * 2; + numberOfBinaryGroupFieldBytesProcessed = 0; + } + + else { + + if (numberOfBinaryGroupFieldBytesProcessed < expectedNumberOfBinaryGroupFieldBytes) { + + /* We have not received all of the expected group fields. */ + + binaryReceiveBuffer[binaryBufferIndex] = curChar; + binaryBufferIndex++; + + numberOfBinaryGroupFieldBytesProcessed++; + + if (numberOfBinaryGroupFieldBytesProcessed == expectedNumberOfBinaryGroupFieldBytes) { + + /* We have all of our group fields. Now we can calculate the expected + length of this packet. */ + + int payloadLength; + + payloadLength = vndevice_computeLengthOfExpectedBinaryPayload(binaryReceiveBuffer); + + expectedBinaryPacketLength = 2 + expectedNumberOfBinaryGroupFieldBytes + payloadLength + 2; + + if (expectedBinaryPacketLength > BINARY_RECEIVE_BUFFER_SIZE) { + + /* This packet will be too large for our buffer + and is likely not a valid packet. */ + binaryBufferIndex = 0; + possibleStartOfBinaryPacketFound = false; + binaryGroupsFieldFound = false; + } + } + } + + else { + + /* Add this byte to the binary buffer. */ + binaryReceiveBuffer[binaryBufferIndex] = curChar; + binaryBufferIndex++; + + /* See if we have reached the end of a binary packet. */ + if (binaryBufferIndex == expectedBinaryPacketLength) { + + /* We might have a packet if we reach here. */ + uint16_t calculatedCrc; + + /* Verify the packet is valid. */ + calculatedCrc = vndevice_checksum_computeCrc16(binaryReceiveBuffer + 1, expectedBinaryPacketLength - 1); + if (calculatedCrc == 0) { + + /* We have a valid binary packet. */ + + vndevice_processReceivedBinaryPacket(vndevice, binaryReceiveBuffer); + + } + else { + + /* We did not have a valid data packet. Let's + process the data we have to see if there are + any packets contained in it. */ + + /* TODO: Reprocess the binary data buffer to see if there is any data in it. */ + } + + /* Reset the search for a new binary packet. */ + binaryBufferIndex = 0; + possibleStartOfBinaryPacketFound = false; + binaryGroupsFieldFound = false; + } + } + } + } + } + } + + vncp_event_signal(vndevice->waitForThreadToStopServicingComPortEvent); + + return VN_NULL; +} + +int vndevice_computeLengthOfExpectedBinaryPayload( + char* ptrToPacketStart) +{ + unsigned char groups = ptrToPacketStart[1]; + int runningPayloadLength = 0; + char* ptrToCurrentGroupField = ptrToPacketStart + 2; + + if (groups & 0x01) { + + /* We have group 1 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(0, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + if (groups & 0x02) { + + /* We have group 2 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(1, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + if (groups & 0x04) { + + /* We have group 3 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(2, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + if (groups & 0x08) { + + /* We have group 4 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(3, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + if (groups & 0x10) { + + /* We have group 5 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(4, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + if (groups & 0x20) { + + /* We have group 6 present. */ + runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(5, *((uint16_t*) ptrToCurrentGroupField)); + + ptrToCurrentGroupField += 2; + } + + return runningPayloadLength; +} + + +void vndevice_processReceivedPacket( + VnDevice* vndevice, + char* buffer) +{ + char responseMatch[VN_RESPONSE_MATCH_SIZE + 1]; + + /* See if we have an error from the sensor. */ + if (strncmp("VNERR", buffer + 1, strlen("VNERR")) == 0) { + + char* result; + + /* Error encountered when trying to get response from the sensor. */ + + result = strtok(buffer, ",*"); /* Returns VNERR */ + result = strtok(0, ",*"); /* Returns error code */ + + vndevice->sensorError = vndevice_convertSensorErrorToSystemError((uint8_t) atoi(result)); + + /* Signal to the user thread we have received a response. */ + if (vndevice_shouldCheckForResponse_threadSafe(vndevice, responseMatch)) { + vndevice_disableResponseChecking_threadSafe(vndevice); + vncp_event_signal(vndevice->waitForCommandResponseEvent); + } + + if (vndevice->errorCodeListener != NULL) + vndevice->errorCodeListener(vndevice->deviceMask, vndevice->sensorError); + } + + /* See if we should be checking for a command response. */ + else if (vndevice_shouldCheckForResponse_threadSafe(vndevice, responseMatch)) { + + /* Does the data packet match the command response we expect? */ + if (strncmp(responseMatch, buffer + 1, strlen(responseMatch)) == 0) { + + /* We found a command response match! */ + + /* If everything checks out on this command packet, let's disable + * further response checking. */ + vndevice_disableResponseChecking_threadSafe(vndevice); + + /* The line below should be thread-safe since the user thread should be + * blocked until we signal that we have received the response. */ + strcpy(vndevice->cmdResponseBuffer, buffer); + + /* Signal to the user thread we have received a response. */ + vncp_event_signal(vndevice->waitForCommandResponseEvent); + } + } + else { + vndevice_processAsyncData(vndevice, buffer); + } +} + +void vndevice_processAsyncData( + VnDevice* vndevice, + char* buffer) +{ + VnDeviceCompositeData data; + char delims[] = ","; + char* result; + + memset(&data, 0, sizeof(VnDeviceCompositeData)); + + if (strncmp(buffer, "$VNYPR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + } + else if (strncmp(buffer, "$VNQTN", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + } + else if (strncmp(buffer, "$VNQTM", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + } + else if (strncmp(buffer, "$VNQTA", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + } + else if (strncmp(buffer, "$VNQTR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNQMA", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + } + else if (strncmp(buffer, "$VNQAR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNQMR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNDCM", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.dcm.c22 = atof(result); + } + else if (strncmp(buffer, "$VNMAG", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + } + else if (strncmp(buffer, "$VNACC", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + } + else if (strncmp(buffer, "$VNGYR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNMAR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNYMR", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNYCM", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.temperature = atof(result); + } + else if (strncmp(buffer, "$VNYBA", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNYIA", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNICM", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + } + else if (strncmp(buffer, "$VNRAW", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.magneticVoltage.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magneticVoltage.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magneticVoltage.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.accelerationVoltage.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.accelerationVoltage.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.accelerationVoltage.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateVoltage.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateVoltage.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateVoltage.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.temperatureVoltage = atof(result); + } + else if (strncmp(buffer, "$VNCMV", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.temperature = atof(result); + } + else if (strncmp(buffer, "$VNSTV", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.quaternion.w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBias.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBias.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBias.c2 = atof(result); + } + else if (strncmp(buffer, "$VNCOV", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.attitudeVariance.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.attitudeVariance.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.attitudeVariance.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBiasVariance.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBiasVariance.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRateBiasVariance.c2 = atof(result); + } + else if (strncmp(buffer, "$VNIMU", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.magnetic.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.acceleration.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.angularRate.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.temperature = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.temperature = atof(result); + } + else if (strncmp(buffer, "$VNGPS", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsTowSec = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsWeek = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsFix = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.numSats = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsVelU = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.timeAccSec = (float) atof(result); + } + else if (strncmp(buffer, "$VNGPE", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsTowSec = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsWeek = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsFix = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.numSats = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.posEcef.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.posEcef.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.posEcef.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velEcef.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velEcef.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velEcef.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsPosU.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsVelU = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.timeAccSec = (float) atof(result); + } + else if (strncmp(buffer, "$VNINS", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsTowSec = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.gpsWeek = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.insStatus = (unsigned short) strtol(result, NULL, 16); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.ypr.roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.latitudeLongitudeAltitude.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velocity.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.attitudeUncertainty = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.posU = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.velU = (float) atof(result); + } + else if (strncmp(buffer, "$VNDTV", 5) == 0) { + + result = strtok(buffer, delims); /* Returns async header. */ + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaTime = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaTheta.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaTheta.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaTheta.c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaVelocity.c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaVelocity.c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return; + data.deltaVelocity.c2 = atof(result); + } + + else { + /* We must not have had an async data packet. */ + return; + } + + /* We had an async data packet and need to move it to vndevice->lastestAsyncData. */ + vncp_criticalSection_enter(&vndevice->critSecForLatestAsyncDataAccess); + memcpy(&vndevice->lastestAsyncData, &data, sizeof(VnDeviceCompositeData)); + vncp_criticalSection_leave(&vndevice->critSecForLatestAsyncDataAccess); + + if (vndevice->asyncDataListener != NULL) + vndevice->asyncDataListener(vndevice->deviceMask, &vndevice->lastestAsyncData); +} + +void vndevice_disableResponseChecking_threadSafe( + VnDevice* vndevice) +{ + vncp_criticalSection_enter(&vndevice->critSecForResponseMatchAccess); + + vndevice->checkForResponse = false; + vndevice->cmdResponseMatchBuffer[0] = 0; + + vncp_criticalSection_leave(&vndevice->critSecForResponseMatchAccess); +} + +bool vndevice_shouldCheckForResponse_threadSafe( + VnDevice* vndevice, + char* responseMatchBuffer) +{ + bool shouldCheckResponse; + + vncp_criticalSection_enter(&vndevice->critSecForResponseMatchAccess); + + shouldCheckResponse = vndevice->checkForResponse; + + if (shouldCheckResponse) + strcpy(responseMatchBuffer, vndevice->cmdResponseMatchBuffer); + + vncp_criticalSection_leave(&vndevice->critSecForResponseMatchAccess); + + return shouldCheckResponse; +} + +VN_ERROR_CODE vndevice_writeSettings( + VnDevice* vndevice, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNWNV"; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSend, "VNWNV"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vndevice_restoreFactorySettings( + VnDevice* vndevice, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNRFS"; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSend, "VNRFS"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vndevice_reset( + VnDevice* vndevice) +{ + return vndevice_writeOutCommand(vndevice, "$VNRST"); +} + +VN_ERROR_CODE vndevice_getBinaryOutputConfiguration( + VnDevice* vndevice, + uint8_t binaryOutputRegisterId, + uint16_t* asyncMode, + uint16_t* rateDivisor, + uint16_t* selectedOutputGroups, + uint16_t* outputGroup1Selections, + uint16_t* outputGroup2Selections, + uint16_t* outputGroup3Selections, + uint16_t* outputGroup4Selections, + uint16_t* outputGroup5Selections, + uint16_t* outputGroup6Selections) +{ + char* result; + int errorCode; + char delims[] = ",*"; + int curBufLoc = 0; + const char* responseMatch = "VNRRG,"; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNRRG,%u", + binaryOutputRegisterId + 74); + cmdToSendBuilder[curBufLoc] = '\0'; + + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *asyncMode = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *rateDivisor = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *selectedOutputGroups = (uint16_t) atoi(result); + + *outputGroup1Selections = 0; + *outputGroup2Selections = 0; + *outputGroup3Selections = 0; + *outputGroup4Selections = 0; + *outputGroup5Selections = 0; + *outputGroup6Selections = 0; + + if (*selectedOutputGroups & 0x01) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup1Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x02) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup2Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x04) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup3Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x08) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup4Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x10) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup5Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x20) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup5Selections = (uint16_t) atoi(result); + } + + if (*selectedOutputGroups & 0x40) { + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *outputGroup6Selections = (uint16_t) atoi(result); + } + + return VNERR_NO_ERROR; +} + +DLL_EXPORT VN_ERROR_CODE vndevice_setBinaryOutputConfiguration( + VnDevice* vndevice, + uint8_t binaryOutputRegisterId, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup4Selections, + uint16_t outputGroup5Selections, + uint16_t outputGroup6Selections, + bool waitForResponse) +{ + int errorCode; + uint16_t outputGroups = 0; + int curBufLoc = 0; + const char* responseMatch = "VNWRG,"; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,%u", + binaryOutputRegisterId + 74); + cmdToSendBuilder[curBufLoc] = '\0'; + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%u", asyncMode); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%u", rateDivisor); + + /* Determine which output groups are selected. */ + if (outputGroup1Selections != 0) { + outputGroups |= 0x01; + } + if (outputGroup2Selections != 0) { + outputGroups |= 0x02; + } + if (outputGroup3Selections != 0) { + outputGroups |= 0x04; + } + if (outputGroup4Selections != 0) { + outputGroups |= 0x08; + } + if (outputGroup5Selections != 0) { + outputGroups |= 0x10; + } + if (outputGroup6Selections != 0) { + outputGroups |= 0x20; + } + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroups); + + /* Now output the available group selections. */ + if (outputGroup1Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup1Selections); + } + if (outputGroup2Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup2Selections); + } + if (outputGroup3Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup3Selections); + } + if (outputGroup4Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup4Selections); + } + if (outputGroup5Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup5Selections); + } + if (outputGroup6Selections != 0) { + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup6Selections); + } + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, responseMatch); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + return VNERR_NO_ERROR; +} + + +VN_ERROR_CODE vndevice_getSynchronizationControl( + VnDevice* vndevice, + uint8_t* syncInMode, + uint8_t* syncInEdge, + uint16_t* syncInSkipFactor, + uint8_t* syncOutMode, + uint8_t* syncOutPolarity, + uint16_t* syncOutSkipFactor, + uint32_t* syncOutPulseWidth) +{ + const char* cmdToSend = "$VNRRG,32"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncInMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncInEdge = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncInSkipFactor = (unsigned short) atoi(result); + result = strtok(0, delims); /* Placeholder for the reserved0 result. */ + if (result == NULL) + return VNERR_INVALID_VALUE; + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncOutMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncOutPolarity = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncOutSkipFactor = (unsigned short) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncOutPulseWidth = (unsigned int) atoi(result); + result = strtok(0, delims); /* Placeholder for the reserved1 result. */ + if (result == NULL) + return VNERR_INVALID_VALUE; + + return VNERR_NO_ERROR; +} + + +VN_ERROR_CODE vndevice_setSynchronizationControl( + VnDevice* vndevice, + uint8_t syncInMode, + uint8_t syncInEdge, + uint16_t syncInSkipFactor, + uint8_t syncOutMode, + uint8_t syncOutPolarity, + uint16_t syncOutSkipFactor, + uint32_t syncOutPulseWidth, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,32,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncInMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncInEdge); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncInSkipFactor); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", 0); /* Placeholder for reserved0 field. */ + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncOutMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncOutPolarity); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncOutSkipFactor); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncOutPulseWidth); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", 0); /* Placeholder for reserved1 field. */ + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getSynchronizationStatus( + VnDevice* vndevice, + uint32_t* syncInCount, + uint32_t* syncInTime, + uint32_t* syncOutCount) +{ + const char* cmdToSend = "$VNRRG,33"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncInCount = (unsigned int) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncInTime = (unsigned int) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *syncOutCount = (unsigned int) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setSynchronizationStatus( + VnDevice* vndevice, + uint32_t syncInCount, + uint32_t syncInTime, + uint32_t syncOutCount, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,33,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncInCount); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncInTime); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", syncOutCount); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getImuMeasurements( + VnDevice* vndevice, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate, + float* temperature, + float* pressure) +{ + const char* cmdToSend = "$VNRRG,54"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *temperature = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *pressure = (float) atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getDeltaThetaAndDeltaVelocity( + VnDevice* vndevice, + float* deltaTime, + VnVector3* deltaTheta, + VnVector3* deltaVelocity) +{ + const char* cmdToSend = "$VNRRG,80"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *deltaTime = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaTheta->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaTheta->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaTheta->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaVelocity->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaVelocity->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + deltaVelocity->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getMagneticCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b) +{ + const char* cmdToSend = "$VNRRG,23"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c22 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setMagneticCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,23,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f", c.c00, c.c01, c.c02, c.c10, c.c11, c.c12, c.c20, c.c21, c.c22); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", b.c0, b.c1, b.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getAccelerationCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b) +{ + const char* cmdToSend = "$VNRRG,25"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c22 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setAccelerationCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,25,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f", c.c00, c.c01, c.c02, c.c10, c.c11, c.c12, c.c20, c.c21, c.c22); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", b.c0, b.c1, b.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getGyroCompensation( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b) +{ + const char* cmdToSend = "$VNRRG,84"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c22 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setGyroCompensation( + VnDevice* vndevice, + VnMatrix3x3 c, + VnVector3 b, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,84,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f", c.c00, c.c01, c.c02, c.c10, c.c11, c.c12, c.c20, c.c21, c.c22); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", b.c0, b.c1, b.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getReferenceFrameRotation( + VnDevice* vndevice, + VnMatrix3x3* c) +{ + const char* cmdToSend = "$VNRRG,26"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c22 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setReferenceFrameRotation( + VnDevice* vndevice, + VnMatrix3x3 c, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,26,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f,%+09.6f", c.c00, c.c01, c.c02, c.c10, c.c11, c.c12, c.c20, c.c21, c.c22); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +DLL_EXPORT VN_ERROR_CODE vndevice_getImuFilteringConfiguration( + VnDevice* vndevice, + uint16_t* magWindowSize, + uint16_t* accelWindowSize, + uint16_t* gyroWindowSize, + uint16_t* tempWindowSize, + uint16_t* presWindowSize, + uint8_t* magFilterMode, + uint8_t* accelFilterMode, + uint8_t* gyroFilterMode, + uint8_t* tempFilterMode, + uint8_t* presFilterMode) +{ + const char* cmdToSend = "$VNRRG,85"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magWindowSize = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelWindowSize = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *gyroWindowSize = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *tempWindowSize = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *presWindowSize = (uint16_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *magFilterMode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelFilterMode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *gyroFilterMode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *tempFilterMode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *presFilterMode = (uint8_t) atoi(result); + + return VNERR_NO_ERROR; +} + +DLL_EXPORT VN_ERROR_CODE vndevice_setImuFilteringConfiguration( + VnDevice* vndevice, + uint16_t magWindowSize, + uint16_t accelWindowSize, + uint16_t gyroWindowSize, + uint16_t tempWindowSize, + uint16_t presWindowSize, + uint8_t magFilterMode, + uint8_t accelFilterMode, + uint8_t gyroFilterMode, + uint8_t tempFilterMode, + uint8_t presFilterMode, + bool waitForResponse) +{ + int errorCode; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + sprintf(cmdToSendBuilder, "$VNWRG,85,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", + magWindowSize, + accelWindowSize, + gyroWindowSize, + tempWindowSize, + presWindowSize, + magFilterMode, + accelFilterMode, + gyroFilterMode, + tempFilterMode, + presFilterMode); + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getDeltaThetaAndDeltaVelocityConfiguration( + VnDevice* vndevice, + uint8_t* integrationFrame, + uint8_t* gyroCompensation, + uint8_t* accelCompensation) +{ + const char* cmdToSend = "$VNRRG,82"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *integrationFrame = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *gyroCompensation = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *accelCompensation = (uint8_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setDeltaThetaAndDeltaVelocityConfiguration( + VnDevice* vndevice, + uint8_t integrationFrame, + uint8_t gyroCompensation, + uint8_t accelCompensation, + bool waitForResponse) +{ + int errorCode; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + sprintf(cmdToSendBuilder, "$VNWRG,82,%d,%d,%d,0,0", + integrationFrame, + gyroCompensation, + accelCompensation); + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getUserTag( + VnDevice* vndevice, + char* userTagBuffer, + uint32_t userTagBufferLength) +{ + const char* cmdToSend = "$VNRRG,0"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + /* Verify the provided buffer is large enough. */ + if (userTagBufferLength < 21) + return VNERR_UNKNOWN_ERROR; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (strlen(result) > 20) + return VNERR_UNKNOWN_ERROR; + strcpy(userTagBuffer, result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setUserTag( + VnDevice* vndevice, + char* userTagData, + uint32_t userTagDataLength, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + /* Verify the provided data is small enough. */ + if (userTagDataLength > 20) + return VNERR_UNKNOWN_ERROR; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,0,"); + + memcpy(cmdToSendBuilder + curBufLoc, userTagData, userTagDataLength); + curBufLoc += userTagDataLength; + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getModelNumber( + VnDevice* vndevice, + char* modelBuffer, + uint32_t modelBufferLength) +{ + const char* cmdToSend = "$VNRRG,1"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + /* Verify the provided buffer is large enough. */ + if (modelBufferLength < 25) + return VNERR_UNKNOWN_ERROR; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (strlen(result) > 24) + return VNERR_UNKNOWN_ERROR; + strcpy(modelBuffer, result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getHardwareRevision( + VnDevice* vndevice, + int32_t* hardwareRevision) +{ + const char* cmdToSend = "$VNRRG,2"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *hardwareRevision = atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getSerialNumber( + VnDevice* vndevice, + char* serialNumberBuffer, + uint32_t serialNumberBufferLength) +{ + const char* cmdToSend = "$VNRRG,3"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + /* Verify the provided buffer is large enough. */ + if (serialNumberBufferLength < 13) + return VNERR_UNKNOWN_ERROR; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (strlen(result) > 12) + return VNERR_UNKNOWN_ERROR; + strcpy(serialNumberBuffer, result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getFirmwareVersion( + VnDevice* vndevice, + char* firmwareVersionBuffer, + uint32_t firmwareVersionBufferLength) +{ + const char* cmdToSend = "$VNRRG,4"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + /* Verify the provided buffer is large enough. */ + if (firmwareVersionBufferLength < 16) + return VNERR_UNKNOWN_ERROR; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (strlen(result) > 15) + return VNERR_UNKNOWN_ERROR; + strcpy(firmwareVersionBuffer, result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getSerialBaudRate( + VnDevice* vndevice, + uint32_t* serialBaudrate) +{ + const char* cmdToSend = "$VNRRG,5"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *serialBaudrate = (uint32_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setSerialBaudRate( + VnDevice* vndevice, + uint32_t serialBaudrate, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,5,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", serialBaudrate); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getAsynchronousDataOutputType( + VnDevice* vndevice, + uint32_t* asyncDataOutputType) +{ + const char* cmdToSend = "$VNRRG,6"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *asyncDataOutputType = (uint32_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setAsynchronousDataOutputType( + VnDevice* vndevice, + uint32_t asyncDataOutputType, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,6,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", asyncDataOutputType); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getAsynchronousDataOutputFrequency( + VnDevice* vndevice, + uint32_t* asyncDataOutputFrequency) +{ + const char* cmdToSend = "$VNRRG,7"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *asyncDataOutputFrequency = (uint32_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setAsynchronousDataOutputFrequency( + VnDevice* vndevice, + uint32_t asyncDataOutputFrequency, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,7,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", asyncDataOutputFrequency); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getQuaternion(VnDevice* vndevice, VnQuaternion* attitude) +{ + const char* cmdToSend = "$VNRRG,9"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getQuaternionMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnQuaternion* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,15"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->x = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->y = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->z = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->w = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getMagnetic( + VnDevice* vndevice, + VnVector3* magnetic) +{ + const char* cmdToSend = "$VNRRG,17"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getAcceleration( + VnDevice* vndevice, + VnVector3* acceleration) +{ + const char* cmdToSend = "$VNRRG,18"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getAngularRate( + VnDevice* vndevice, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,19"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,20"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getYawPitchRoll( + VnDevice* vndevice, + VnYpr* attitude) +{ + const char* cmdToSend = "$VNRRG,8"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->roll = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getYawPitchRollMagneticAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* magnetic, + VnVector3* acceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,27"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magnetic->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + acceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getYawPitchRollTrueBodyAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* bodyAcceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,239"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + bodyAcceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + bodyAcceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + bodyAcceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getYawPitchRollTrueInertialAccelerationAngularRate( + VnDevice* vndevice, + VnYpr* attitude, + VnVector3* inertialAcceleration, + VnVector3* angularRate) +{ + const char* cmdToSend = "$VNRRG,240"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->yaw = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->pitch = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + attitude->roll = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + inertialAcceleration->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + angularRate->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getVpeControl( + VnDevice* vndevice, + uint8_t* enable, + uint8_t* headingMode, + uint8_t* filteringMode, + uint8_t* tuningMode) +{ + const char* cmdToSend = "$VNRRG,35"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *enable = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *headingMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *filteringMode = (unsigned char) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *tuningMode = (unsigned char) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setVpeControl( + VnDevice* vndevice, + uint8_t enable, + uint8_t headingMode, + uint8_t filteringMode, + uint8_t tuningMode, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,35,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", enable); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", headingMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", filteringMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", tuningMode); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getVpeMagnetometerBasicTuning( + VnDevice* vndevice, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering) +{ + const char* cmdToSend = "$VNRRG,36"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setVpeMagnetometerBasicTuning( + VnDevice* vndevice, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,36,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", baseTuning.c0, baseTuning.c1, baseTuning.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", adaptiveTuning.c0, adaptiveTuning.c1, adaptiveTuning.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", adaptiveFiltering.c0, adaptiveFiltering.c1, adaptiveFiltering.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getVpeAccelerometerBasicTuning( + VnDevice* vndevice, + VnVector3* baseTuning, + VnVector3* adaptiveTuning, + VnVector3* adaptiveFiltering) +{ + const char* cmdToSend = "$VNRRG,38"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + baseTuning->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveTuning->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + adaptiveFiltering->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setVpeAccelerometerBasicTuning( + VnDevice* vndevice, + VnVector3 baseTuning, + VnVector3 adaptiveTuning, + VnVector3 adaptiveFiltering, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,38,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", baseTuning.c0, baseTuning.c1, baseTuning.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", adaptiveTuning.c0, adaptiveTuning.c1, adaptiveTuning.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", adaptiveFiltering.c0, adaptiveFiltering.c1, adaptiveFiltering.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getMagnetometerCalibrationControl( + VnDevice* vndevice, + uint8_t* hsiMode, + uint8_t* hsiOutput, + uint8_t* convergeRate) +{ + const char* cmdToSend = "$VNRRG,44"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *hsiMode = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *hsiOutput = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *convergeRate = (uint8_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setMagnetometerCalibrationControl( + VnDevice* vndevice, + uint8_t hsiMode, + uint8_t hsiOutput, + uint8_t convergeRate, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,44,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", hsiMode); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", hsiOutput); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", convergeRate); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getCalculatedMagnetometerCalibration( + VnDevice* vndevice, + VnMatrix3x3* c, + VnVector3* b) +{ + const char* cmdToSend = "$VNRRG,47"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c00 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c01 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c02 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c10 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c11 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c12 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c20 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c21 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + c->c22 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + b->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_getMagneticGravityReferenceVectors( + VnDevice* vndevice, + VnVector3* magneticReference, + VnVector3* gravityReference) +{ + const char* cmdToSend = "$VNRRG,21"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticReference->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticReference->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + magneticReference->c2 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gravityReference->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gravityReference->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + gravityReference->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setMagneticGravityReferenceVectors( + VnDevice* vndevice, + VnVector3 magneticReference, + VnVector3 gravityReference, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,21,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", magneticReference.c0, magneticReference.c1, magneticReference.c2); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%+09.6f,%+09.6f,%+09.6f", gravityReference.c0, gravityReference.c1, gravityReference.c2); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getCommunicationProtocolControl( + VnDevice* vndevice, + uint8_t* serialCount, + uint8_t* serialStatus, + uint8_t* spiCount, + uint8_t* spiStatus, + uint8_t* serialChecksum, + uint8_t* spiChecksum, + uint8_t* errorMode) +{ + const char* cmdToSend = "$VNRRG,30"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *serialCount = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *serialStatus = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *spiCount = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *spiStatus = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *serialChecksum = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *spiChecksum = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *errorMode = (uint8_t) atoi(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setCommunicationProtocolControl( + VnDevice* vndevice, + uint8_t serialCount, + uint8_t serialStatus, + uint8_t spiCount, + uint8_t spiStatus, + uint8_t serialChecksum, + uint8_t spiChecksum, + uint8_t errorMode, + bool waitForResponse) +{ + int errorCode; + int curBufLoc = 0; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + curBufLoc = sprintf(cmdToSendBuilder, "$VNWRG,30,"); + + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", serialCount); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", serialStatus); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", spiCount); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", spiStatus); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", serialChecksum); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", spiChecksum); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ","); + curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, "%d", errorMode); + + cmdToSendBuilder[curBufLoc] = '\0'; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_getReferenceVectorConfiguration( + VnDevice* vndevice, + uint8_t* useMagModel, + uint8_t* useGravityModel, + uint32_t* recalcThreshold, + float* year, + VnVector3* lla) +{ + const char* cmdToSend = "$VNRRG,83"; + char delims[] = ",*"; + char* result; + int errorCode; + const char* responseMatch = "VNRRG,"; + + errorCode = vndevice_transaction(vndevice, cmdToSend, responseMatch); + + if (errorCode != VNERR_NO_ERROR) + return errorCode; + + result = strtok(vndevice->cmdResponseBuffer, delims); /* Returns VNRRG */ + result = strtok(0, delims); /* Returns register ID */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *useMagModel = (uint8_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *useGravityModel = (uint8_t) atoi(result); + result = strtok(0, delims); /* Placeholder for Resv1 field. */ + result = strtok(0, delims); /* Placeholder for Resv2 field. */ + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *recalcThreshold = (uint32_t) atoi(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + *year = (float) atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lla->c0 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lla->c1 = atof(result); + result = strtok(0, delims); + if (result == NULL) + return VNERR_INVALID_VALUE; + lla->c2 = atof(result); + + return VNERR_NO_ERROR; +} + +VN_ERROR_CODE vndevice_setReferenceVectorConfiguration( + VnDevice* vndevice, + uint8_t useMagModel, + uint8_t useGravityModel, + uint32_t recalcThreshold, + float year, + VnVector3 lla, + bool waitForResponse) +{ + int errorCode; + char cmdToSendBuilder[VN_MAX_COMMAND_SIZE]; + + sprintf(cmdToSendBuilder, "$VNWRG,83,%d,%d,%d,0,0,%+09.6f,%+09.6f,%+09.6f,%+09.6f", + useMagModel, + useGravityModel, + recalcThreshold, + year, + lla.c0, + lla.c1, + lla.c2); + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, "VNWRG,"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSendBuilder); + + return errorCode; +} + +VN_ERROR_CODE vndevice_pauseAsyncOutputs( + VnDevice* vndevice, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNASY,0"; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSend, "VNASY"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSend); + + return errorCode; +} + +VN_ERROR_CODE vndevice_resumeAsyncOutputs( + VnDevice* vndevice, + bool waitForResponse) +{ + int errorCode; + const char* cmdToSend = "$VNASY,1"; + + if (waitForResponse) + errorCode = vndevice_transaction(vndevice, cmdToSend, "VNASY"); + else + errorCode = vndevice_writeOutCommand(vndevice, cmdToSend); + + return errorCode; +} + +/** \endcond */